A mapping device, method, apparatus, and readable storage medium of a construction machine

Abstract

The application provides a mapping method, equipment and readable storage medium of an engineering machine, and the technical scheme is as follows: IMU data and IMU odometry are used to correct motion distortion of a current frame laser point cloud, point lines and point surface features are extracted according to the curvature of each point of the current frame laser point cloud, a local map is matched with the current frame laser point cloud, and factor graph optimization is performed on a key frame. The output laser odometry is used to correct IMU zero offset, the IMU pre-integrator is reset, and the IMU odometry is output. When the current frame laser point cloud meets a loop detection condition, a normal distribution transformation algorithm is used to perform loop detection on the current frame laser point cloud, and a global pose is updated, so that the mapping problem in the degradation scene of the engineering machine is solved, and fast mapping and positioning in a large scene are realized.

Description

Technical Field

[0001] This invention relates to the field of motion mapping, and in particular to a mapping method, apparatus, and readable storage medium for engineering machinery. Background Technology

[0002] Construction machinery has a wide range of applications in the construction industry. With the continuous development of global technology, intelligent construction machinery has become a future development trend. Intelligent construction machinery includes intelligent products and construction, intelligent manufacturing, intelligent services, and intelligent management. Unmanned construction machinery falls under the category of intelligent products and construction, which refers to the ability of construction machinery to autonomously complete corresponding walking and operation tasks without human intervention.

[0003] Generally, unmanned construction machinery is considered to consist of five major modules: environmental perception, vehicle positioning, decision-making and planning, motion control, and automated operation. Among these, vehicle positioning is the foundation for realizing decision-making and planning, motion control, and automated operation. Unlike ordinary passenger vehicles, construction machinery operates under complex conditions and harsh environments, and a single sensor cannot meet the high-precision and high-robustness positioning requirements of the entire construction machinery vehicle.

[0004] Currently, autonomous vehicle technology is mainly applied in the automotive field, with relatively little research on related technologies in the construction machinery field. Existing autonomous construction machinery vehicle positioning technologies are mostly based on single-sensor positioning, which suffers from problems such as slow mapping speed, easy mapping degradation, and low positioning robustness.

[0005] In view of the above, this application is hereby submitted. Summary of the Invention

[0006] This invention discloses a drawing, method, equipment, and readable storage medium for engineering machinery, aiming to solve problems such as slow drawing speed, easy degradation of drawings, and low positioning robustness in engineering machinery.

[0007] The first embodiment of the present invention provides a method for drawing engineering machinery, comprising:

[0008] The environmental information collected by the vehicle-mounted multi-sensor fusion platform is acquired, and motion distortion correction is performed on the current frame laser point cloud of the environmental information using IMU data and IMU odometry.

[0009] Based on the curvature of each point in the current frame's laser point cloud after correction, point-line features and point-surface features are extracted, and the point-line features and point-surface features are matched with the local map;

[0010] Determine whether the current frame laser point cloud is a keyframe;

[0011] If not, correct the IMU bias and reset the IMU pre-integrator;

[0012] If so, factor map optimization is performed on the laser point cloud of the current frame, and the laser point cloud of the current frame is registered with the local map composed of historical frames using the normal distribution transformation algorithm to optimize and correct the global pose.

[0013] Preferably, the step of acquiring environmental information collected by the vehicle-mounted multi-sensor fusion platform and using IMU data and IMU odometry to perform motion distortion correction on the current frame laser point cloud of the environmental information specifically involves:

[0014] The IMU data is pre-integrated to obtain the coordinate transformation matrix of the lidar from the end time of the current frame laser point cloud scanning to the start time of the scanning.

[0015] Motion distortion correction is performed on the current frame laser point cloud using the coordinate transformation matrix; wherein, the coordinate transformation matrix is:

[0016] ;

[0017]

[0018] in, This is the coordinate transformation matrix of each point in the current frame of the laser point cloud relative to the initial scanning time. Let be the coordinate transformation matrix of the lidar from the end of the scan to the start of the scan. The measurement time of each point in the laser point cloud in the current frame within the scanning cycle. The scan start time, The end time of the scan. These are the coordinates of each point in the current frame of the laser point cloud in the lidar coordinate system at the start of the scan. This represents the coordinates of each point in the laser point cloud of the current frame in the lidar coordinate system at the current moment.

[0019] Preferably, the step of extracting point-line features and point-surface features based on the curvature of each point in the corrected current frame laser point cloud, and matching the point-line features and point-surface features with the local map, specifically involves:

[0020] The curvature calculation model is invoked to generate the curvature of each point in the laser point cloud of the current frame. The curvature calculation model is as follows:

[0021]

[0022] in, For laser point cloud The curvature of a point For the first The distance from the point to the origin of the lidar coordinate system. For the first The distances of the five laser points before and after the point from the origin of the lidar coordinate system;

[0023] The laser point cloud of the current frame is subjected to feature extraction and division according to a preset curvature threshold. Points in the laser point cloud of the current frame whose curvature is greater than the curvature threshold are determined to be corner points, and those whose curvature is less than the curvature threshold are determined to be face points.

[0024] Voxel filtering is performed on the corner points and face points to generate the feature point cloud of the current frame;

[0025] The kd-tree is used to find the points in the local map that are closest to the corner features in the current frame feature point cloud and connect them to form a line. The distance from the corner point in the current frame feature point cloud to this line is then generated. The specific calculation model is as follows:

[0026] ;

[0027] in, for The first time in the feature point cloud of the current frame at time step The coordinates of each corner point in the world coordinate system and For the first The coordinates of each corner point in two world coordinate systems on the corresponding edge feature in the local map;

[0028] For the polygon features in the current frame's feature point cloud, the kd-tree is used to find the five closest points in the local map, and the following plane equation is constructed:

[0029] ;

[0030] Where A, B, C, and D are the coefficients of the plane equation, and x, y, and z are the coordinates of the five points in the local map that are most similar to the feature point cloud features of the current frame.

[0031] Substitute the coordinates of five points corresponding to the feature point cloud features of the current frame in the local map into the plane equation to obtain the coefficients of the plane equation. Then, calculate the distance from the feature point cloud features of the current frame to the feature plane of the local map based on the coefficients of the plane equation. The calculation model is as follows:

[0032]

[0033] in, These are the coordinates of the face points in the current frame's feature point cloud in the world coordinate system.

[0034] Based on the distances between the corner points of the current frame laser point cloud and the corresponding edge features of the local map, and the distances between the surface points of the current frame laser point cloud and the corresponding planar features of the local map, the following residual equation is constructed:

[0035]

[0036]

[0037] in, For the current frame feature point cloud, the first The coordinates of a point in the world coordinate system For the current frame feature point cloud, the first The coordinates of the point in the lidar coordinate system For the current frame feature point cloud, the first The coordinate transformation matrix for transforming a point from the lidar coordinate system to the world coordinate system. This is a local map in the world coordinate system. and These are the feature point clouds of the current frame, numbered... The rotation and translation matrices for transforming a point from the lidar coordinate system to the world coordinate system;

[0038] Using the residual equation and the Levember algorithm, the following iterative equation is constructed:

[0039]

[0040]

[0041] in, Let be the Jacobian matrix of the residual equation with respect to the iteration variables. For the diagonal update operator, It is the identity matrix. For the increment of the variable in a single iteration, The residual equation is... For the current frame feature point cloud, the first The coordinates of the point in the lidar coordinate system For the current frame feature point cloud, the first The coordinate transformation matrix for transforming a point from the lidar coordinate system to the world coordinate system;

[0042] The sum of residuals from feature matching is calculated using the following model:

[0043] ;

[0044] in, For the current frame feature point cloud, the first The residual values ​​between each corner point and the corresponding edge features of the local map. For the current frame feature point cloud, the first The residual values ​​between a point and the corresponding planar features of the local map. and These represent the number of corner points and face points in the feature point cloud of the current frame, respectively.

[0045] The diagonal update operator is updated based on the sum of residuals from feature matching. If the sum of residuals is too large, the diagonal operator is increased; otherwise, the diagonal operator is decreased.

[0046] Preferably, the step of correcting the IMU zero bias and resetting the IMU pre-integrator specifically involves:

[0047] Define a pre-integrator for optimizing IMU zero bias and a pre-integrator for calculating the IMU odometer reading, respectively.

[0048] Acquire laser odometry data and update the factor graph. Determine if the factor tree in the updated factor graph exceeds a preset value. If it does, reconstruct the factor graph.

[0049] Add the IMU raw data pre-integration factor, zero bias factor, and current frame laser odometry factor between the current frame laser point cloud and the previous frame laser point cloud to the updated factor graph;

[0050] The pose and velocity estimated by the pre-integrator used to optimize the IMU zero bias are used as the estimated values ​​of the pose factor and velocity factor of the current frame. The zero bias optimized in the previous frame is used as the estimated value of the zero bias factor of the current frame. Factor graph optimization is performed to generate the optimized and updated velocity and zero bias.

[0051] Using the optimized and updated zero bias, reset the pre-integrator used to optimize the IMU zero bias;

[0052] The optimized and updated speed and zero bias are tested. If the test requirements are not met, the factor graph is reconstructed for optimization.

[0053] The optimized and updated zero-bias resets the pre-integrator used to calculate the IMU odometry, and pre-integrates the raw IMU data after the current frame laser point cloud time to obtain the pre-integration result;

[0054] Using the state of the laser point cloud at the current frame moment obtained by the factor graph optimization and the IMU zero bias, combined with the pre-integration result, the state of the laser point cloud after the current frame moment is estimated and output as a high-frequency odometry.

[0055] Preferably, the step of performing factor map optimization on the current frame laser point cloud and registering the current frame laser point cloud with the local map composed of historical frames using a normal distribution transformation algorithm to optimize and correct the global pose specifically includes:

[0056] When the current frame of the laser point cloud is determined to be a key frame, the loop closure detection thread is invoked based on the laser odometry.

[0057] The loop closure detection thread determines whether there are any historical keyframes in the historical keyframe set that are similar to the current keyframe.

[0058] If so, determine whether the timestamp interval between the current keyframe and the similar historical keyframe is greater than a threshold. If it is greater than the threshold, use the normal distribution transformation algorithm to match the current keyframe with the similar historical keyframe and generate a relative coordinate transformation matrix.

[0059] The global pose is optimized and corrected using the relative coordinate transformation matrix.

[0060] Preferably, the step of using a normal distribution transformation algorithm to match the current keyframe with similar historical keyframes to generate a relative coordinate transformation matrix specifically involves:

[0061] By rasterizing similar historical keyframes into target point clouds, and assuming that each grid conforms to a normal distribution, the probability density function of the laser point cloud distribution in each grid is calculated. The calculation model is as follows:

[0062]

[0063] in, Let the dimension be the vector. It is the mean vector. Let be the covariance matrix of a random vector;

[0064] The target point cloud is placed into each grid, and the mean value of the laser point cloud data in each grid is calculated. The calculation model is as follows:

[0065]

[0066] in, The coordinates of all points in the point cloud within a single grid cell;

[0067] The covariance of the point cloud data in each grid is calculated using the covariance calculation formula. The calculation model is as follows:

[0068]

[0069] The current keyframe is placed into each grid as the source point cloud for point cloud registration using the normal distribution transform algorithm. The probability of each point in the source point cloud in each grid is calculated, and the calculation model is as follows:

[0070]

[0071] The maximum likelihood function is obtained by multiplying the probabilities of each point in the source point cloud in each grid cell. When the maximum likelihood function reaches its maximum value, the relative coordinate transformation matrix is ​​the optimal solution. The calculation model is as follows:

[0072]

[0073]

[0074] in, Let be the rotation matrix of the source point cloud relative to the target point cloud. Let be the translation matrix of the source point cloud relative to the target point cloud. Let be the coordinate transformation matrix of the source point cloud relative to the target point cloud. The coordinates of each point in the source point cloud are given in the coordinate system of the target point cloud. The coordinates of each point in the source point cloud in its own coordinate system;

[0075] Taking the logarithm of both sides of the maximum likelihood function, its computational model is as follows:

[0076]

[0077] The maximum likelihood function is solved using Newton's iteration method. The coordinate transformation matrix of the source point cloud relative to the target point cloud is used as the iteration variable. The coordinate transformation matrix corresponding to the maximum likelihood function is solved by continuously updating the iteration variable along the gradient descent direction. The calculation model is as follows:

[0078]

[0079]

[0080] in, The Hessian matrix is ​​the maximum likelihood function. Let be the gradient vector of the maximum likelihood function. This is the coordinate transformation matrix of the source point cloud relative to the target point cloud.

[0081] The second embodiment of the present invention provides a mapping device for engineering machinery, comprising:

[0082] The distortion correction unit is used to acquire environmental information collected by the vehicle-mounted multi-sensor fusion platform, and to perform motion distortion correction on the current frame laser point cloud of the environmental information using IMU data and IMU odometry.

[0083] The map matching unit is used to extract point-line features and point-surface features based on the curvature of each point in the current frame laser point cloud after correction, and to match the point-line features and point-surface features with the local map;

[0084] The judgment unit is used to determine whether the current frame laser point cloud is a key frame;

[0085] The IMU calibration unit is used to calibrate the IMU zero bias and reset the IMU pre-integrator.

[0086] The global optimization unit is used to perform factor map optimization on the laser point cloud of the current frame, and to register the laser point cloud of the current frame with the local map composed of historical frames using a normal distribution transformation algorithm, thereby optimizing and correcting the global pose.

[0087] The third embodiment of the present invention provides a drawing device for engineering machinery, including a memory and a processor. The memory stores a computer program, which can be executed by the processor to implement a drawing method for engineering machinery as described in any of the above embodiments.

[0088] The fourth embodiment of the present invention provides a computer-readable storage medium, characterized in that it stores a computer program, which can be executed by the processor of the device where the computer-readable storage medium is located, to implement a drawing method for engineering machinery as described in any of the above claims.

[0089] The key technical point of the mapping method, device, and readable storage medium for engineering machinery provided by this invention is to use IMU data and IMU odometry to correct motion distortion of the current frame laser point cloud, extract point-line and point-surface features based on the curvature of each point in the current frame laser point cloud, match the local map with the current frame laser point cloud, and optimize the key frames using factor maps. The IMU zero bias is corrected based on the output laser odometry, the IMU pre-integrator is reset, and the output IMU odometry is used. When the current frame laser point cloud meets the loop closure detection condition, a normal distribution transformation algorithm is used to perform loop closure detection on the current frame laser point cloud and update the global pose. This solves the mapping problem in degraded engineering machinery scenarios and achieves rapid mapping and localization in large scenes. Attached Figure Description

[0090] Figure 1 This is a flowchart illustrating a method for drawing engineering machinery according to the first embodiment of the present invention;

[0091] Figure 2 This is a schematic diagram of a mapping device for engineering machinery provided in the second embodiment of the present invention. Detailed Implementation

[0092] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0093] To better understand the technical solution of the present invention, the embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0094] It should be understood that the described embodiments are merely some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0095] The terminology used in the embodiments of this invention is for the purpose of describing particular embodiments only and is not intended to limit the invention. The singular forms “a,” “the,” and “the” as used in the embodiments of this invention and the appended claims are also intended to include the plural forms unless the context clearly indicates otherwise.

[0096] It should be understood that the term "and / or" used in this article is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, and B existing alone. Additionally, the character " / " in this article generally indicates that the preceding and following related objects have an "or" relationship.

[0097] Depending on the context, the word "if" as used here can be interpreted as "when," "when," "in response to determination," or "in response to detection." Similarly, depending on the context, the phrase "if determination" or "if detection (of the stated condition or event)" can be interpreted as "when determination," "in response to determination," "when detection (of the stated condition or event)," or "in response to detection (of the stated condition or event)."

[0098] The terms "first" and "second" used in the embodiments are merely to distinguish similar objects and do not represent a specific ordering of objects. It is understood that "first" and "second" can be interchanged in a specific order or sequence where permissible. It should be understood that the objects distinguished by "first" and "second" can be interchanged where appropriate so that the embodiments described herein can be implemented in orders other than those illustrated or described herein.

[0099] The specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0100] This invention discloses a drawing, method, equipment, and readable storage medium for engineering machinery, aiming to solve problems such as slow drawing speed, easy degradation of drawings, and low positioning robustness in engineering machinery.

[0101] Please see Figure 1The first embodiment of the present invention provides a drawing method for engineering machinery, which can be executed by a drawing device for engineering machinery, specifically by one or more processors within the drawing device, to at least implement the following steps:

[0102] S101, acquire environmental information collected by the vehicle-mounted multi-sensor fusion platform, and use IMU data and IMU odometry to perform motion distortion correction on the current frame laser point cloud of the environmental information;

[0103] In this embodiment, the mapping device can be a terminal with data processing and analysis capabilities, such as a desktop computer, laptop computer, server, or workstation. The mapping device can be equipped with a corresponding operating system and application software, and the functions required in this embodiment are realized through the combination of the operating system and application software. The multi-sensor fusion platform can include sensing devices such as IMU sensors, lidar, and laser odometry.

[0104] It should be noted that a single frame of point cloud data from a LiDAR scanner is obtained by scanning one full circle (360°), but the vehicle is also moving at high speed during that scan cycle. This results in inaccurate point cloud coordinates obtained from different scanning angles in the initial coordinate system of the scan.

[0105] Specifically, in this embodiment, the IMU data can be pre-integrated to obtain the coordinate transformation matrix of the lidar from the end of the current frame laser point cloud scanning time to the start of the scanning time. The transformation matrix is ​​equivalent to the rotation and translation of the vehicle at each scanning time relative to the start of the scanning time. Then, the rotation and translation are converted into 4... The homogeneous coordinate transformation matrix of 4.

[0106] Motion distortion correction is performed on the current frame laser point cloud using the coordinate transformation matrix; wherein, the coordinate transformation matrix is:

[0107] ;

[0108]

[0109] in, This is the coordinate transformation matrix of each point in the current frame of the laser point cloud relative to the initial scanning time. Let be the coordinate transformation matrix of the lidar from the end of the scan to the start of the scan. The measurement time of each point in the laser point cloud in the current frame within the scanning cycle. The scan start time, The end time of the scan. These are the coordinates of each point in the current frame of the laser point cloud in the lidar coordinate system at the start of the scan. This represents the coordinates of each point in the laser point cloud of the current frame in the lidar coordinate system at the current moment.

[0110] S102, extract point-line features and point-surface features based on the curvature of each point in the current frame laser point cloud after correction, and match the point-line features and point-surface features with the local map;

[0111] It should be noted that the map itself is composed of features extracted from each frame. The role of matching is to obtain the coordinate transformation matrix of the current frame relative to the map. Using this coordinate transformation matrix, the current frame can be accurately stitched onto the global map.

[0112] Specifically, in this embodiment,

[0113] The curvature calculation model is invoked to generate the curvature of each point in the laser point cloud of the current frame. The curvature calculation model is as follows:

[0114]

[0115] in, For laser point cloud The curvature of a point For the first The distance from the point to the origin of the lidar coordinate system. For the first The distances of the five laser points before and after the point from the origin of the lidar coordinate system;

[0116] The laser point cloud of the current frame is subjected to feature extraction and division according to a preset curvature threshold. Points in the laser point cloud of the current frame whose curvature is greater than the curvature threshold are determined to be corner points, and those whose curvature is less than the curvature threshold are determined to be face points.

[0117] Voxel filtering is performed on the corner points and face points to generate the feature point cloud of the current frame;

[0118] The kd-tree is used to find the points in the local map that are closest to the corner features in the current frame feature point cloud and connect them to form a line. The distance from the corner point in the current frame feature point cloud to this line is then generated. The specific calculation model is as follows:

[0119] ;

[0120] in, for The first time in the feature point cloud of the current frame at time step The coordinates of each corner point in the world coordinate system and For the first The coordinates of each corner point in two world coordinate systems on the corresponding edge feature in the local map;

[0121] For the polygon features in the current frame's feature point cloud, the kd-tree is used to find the five closest points in the local map, and the following plane equation is constructed:

[0122] ;

[0123] Where A, B, C, and D are the coefficients of the plane equation, and x, y, and z are the coordinates of the five points in the local map that are most similar to the feature point cloud features of the current frame.

[0124] Substitute the coordinates of five points corresponding to the feature point cloud features of the current frame in the local map into the plane equation to obtain the coefficients of the plane equation. Then, calculate the distance from the feature point cloud features of the current frame to the feature plane of the local map based on the coefficients of the plane equation. The calculation model is as follows:

[0125]

[0126] in, These are the coordinates of the face points in the current frame's feature point cloud in the world coordinate system.

[0127] Based on the distances between the corner points of the current frame laser point cloud and the corresponding edge features of the local map, and the distances between the surface points of the current frame laser point cloud and the corresponding planar features of the local map, the following residual equation is constructed:

[0128]

[0129]

[0130] in, For the current frame feature point cloud, the first The coordinates of a point in the world coordinate system For the current frame feature point cloud, the first The coordinates of the point in the lidar coordinate system For the current frame feature point cloud, the first The coordinate transformation matrix for transforming a point from the lidar coordinate system to the world coordinate system. This is a local map in the world coordinate system. and These are the feature point clouds of the current frame. The rotation and translation matrices for transforming a point from the lidar coordinate system to the world coordinate system;

[0131] Using the residual equation and the Levember algorithm, the following iterative equation is constructed:

[0132]

[0133]

[0134] in, Let be the Jacobian matrix of the residual equation with respect to the iteration variables. For the diagonal update operator, It is the identity matrix. For the increment of the variable in a single iteration, The residual equation is... For the current frame feature point cloud, the first The coordinates of the point in the lidar coordinate system For the current frame feature point cloud, the first The coordinate transformation matrix for transforming a point from the lidar coordinate system to the world coordinate system;

[0135] The sum of residuals from feature matching is calculated using the following model:

[0136] ;

[0137] in, For the current frame feature point cloud, the first The residual values ​​between each corner point and the corresponding edge features of the local map. For the current frame feature point cloud, the first The residual values ​​between a point and the corresponding planar features of the local map. and These represent the number of corner points and face points in the feature point cloud of the current frame, respectively.

[0138] The diagonal update operator is updated based on the sum of residuals from feature matching. If the sum of residuals is too large, the diagonal operator is increased; otherwise, the diagonal operator is decreased.

[0139] S103, determine whether the current frame laser point cloud is a key frame;

[0140] It should be noted that the keyframe is determined by distance. For example, if the distance between the current frame and the previous keyframe exceeds d, the current frame is determined to be a keyframe. In this embodiment, the keyframe can be determined by the data collected by the laser odometry. Of course, in other embodiments, other methods can be used to determine the keyframe. No specific limitation is made here, but these solutions are all within the protection scope of this invention.

[0141] S104, when it is determined that the current frame laser point cloud is a non-critical frame, the IMU zero bias is corrected and the IMU pre-integrator is reset;

[0142] Define a pre-integrator for optimizing IMU zero bias and a pre-integrator for calculating the IMU odometer reading, respectively.

[0143] Acquire the laser odometry data and update the factor graph. Determine whether the factor tree in the updated factor graph exceeds a preset value (the preset value can be, but is not limited to, 100). If it does, reconstruct the factor graph.

[0144] Add the IMU raw data pre-integration factor, zero bias factor, and current frame laser odometry factor between the current frame laser point cloud and the previous frame laser point cloud to the updated factor graph;

[0145] The pose and velocity estimated by the pre-integrator used to optimize the IMU zero bias are used as the estimated values ​​of the pose factor and velocity factor of the current frame. The zero bias optimized in the previous frame is used as the estimated value of the zero bias factor of the current frame. Factor graph optimization is performed to generate the optimized and updated velocity and zero bias.

[0146] Using the optimized and updated zero bias, reset the pre-integrator used to optimize the IMU zero bias;

[0147] The optimized and updated speed and zero bias are tested. If the test requirements are not met, the factor graph is reconstructed for optimization.

[0148] The optimized and updated zero-bias resets the pre-integrator used to calculate the IMU odometry, and pre-integrates the raw IMU data after the current frame laser point cloud time to obtain the pre-integration result;

[0149] Using the state of the laser point cloud at the current frame moment obtained by the factor graph optimization and the IMU zero bias, combined with the pre-integration result, the state of the laser point cloud after the current frame moment is estimated and output as a high-frequency odometry.

[0150] S105, when it is determined that the current frame laser point cloud is a key frame, factor map optimization is performed on the current frame laser point cloud, and the local map composed of the current frame laser point cloud and the historical frames is registered using the normal distribution transformation algorithm to optimize and correct the global pose.

[0151] Specifically, in this embodiment:

[0152] When the current frame of the laser point cloud is determined to be a key frame, the loop closure detection thread is called based on the laser odometry. It should be noted that even when the current frame is detected to be a key frame, the decision to call the loop closure detection thread still needs to be based on the laser odometry to avoid performing loop closure detection on every key frame, which could lead to insufficient computing power.

[0153] The loop closure detection thread determines whether there are any historical keyframes in the historical keyframe set that are similar to the current keyframe.

[0154] If so, determine whether the timestamp interval between the current keyframe and the similar historical keyframe is greater than a threshold. If it is greater than the threshold, use the normal distribution transformation algorithm to match the current keyframe with the similar historical keyframe and generate a relative coordinate transformation matrix.

[0155] The global pose is optimized and corrected using the relative coordinate transformation matrix.

[0156] In one possible embodiment of the present invention, the step of using a normal distribution transformation algorithm to match the current keyframe with similar historical keyframes to generate a relative coordinate transformation matrix specifically involves:

[0157] By rasterizing similar historical keyframes as target point clouds, and assuming that each grid conforms to a normal distribution, the probability density function of the laser point cloud distribution of each grid is calculated. The calculation model is as follows:

[0158]

[0159] in, Let the dimension be the vector. It is the mean vector. Let be the covariance matrix of a random vector;

[0160] The target point cloud is placed into each grid, and the mean value of the laser point cloud data in each grid is calculated. The calculation model is as follows:

[0161]

[0162] in, The coordinates of all points in the point cloud within a single grid cell;

[0163] The covariance of the point cloud data in each grid is calculated using the covariance calculation formula. The calculation model is as follows:

[0164]

[0165] The current keyframe is placed into each grid as the source point cloud for point cloud registration using the normal distribution transform algorithm. The probability of each point in the source point cloud in each grid is calculated, and the calculation model is as follows:

[0166]

[0167] The maximum likelihood function is obtained by multiplying the probabilities of each point in the source point cloud in each grid cell. When the maximum likelihood function reaches its maximum value, the relative coordinate transformation matrix is ​​the optimal solution. The calculation model is as follows:

[0168]

[0169]

[0170] in, Let be the rotation matrix of the source point cloud relative to the target point cloud. Let be the translation matrix of the source point cloud relative to the target point cloud. Let be the coordinate transformation matrix of the source point cloud relative to the target point cloud. These are the coordinates of each point in the source point cloud in the target point cloud coordinate system. The coordinates of each point in the source point cloud in its own coordinate system;

[0171] Taking the logarithm of both sides of the maximum likelihood function, its computational model is as follows:

[0172]

[0173] The maximum likelihood function is solved using Newton's iteration method. The coordinate transformation matrix of the source point cloud relative to the target point cloud is used as the iteration variable. The coordinate transformation matrix corresponding to the maximum likelihood function is solved by continuously updating the iteration variable along the gradient descent direction. The calculation model is as follows:

[0174]

[0175]

[0176] in, The Hessian matrix is ​​the maximum likelihood function. Let be the gradient vector of the maximum likelihood function. This is the coordinate transformation matrix of the source point cloud relative to the target point cloud.

[0177] Please see Figure 2 The second embodiment of the present invention provides a mapping device for engineering machinery, comprising:

[0178] The distortion correction unit 201 is used to acquire environmental information collected by the vehicle-mounted multi-sensor fusion platform, and to perform motion distortion correction on the current frame laser point cloud of the environmental information using IMU data and IMU odometry.

[0179] The map matching unit 202 is used to extract point-line features and point-surface features based on the curvature of each point in the current frame laser point cloud after correction, and to match the point-line features and point-surface features with the local map;

[0180] The judgment unit 203 is used to determine whether the current frame laser point cloud is a key frame;

[0181] IMU calibration unit 204 is used to calibrate the IMU zero bias and reset the IMU pre-integrator;

[0182] The global optimization unit 205 is used to perform factor map optimization on the laser point cloud of the current frame, and to register the laser point cloud of the current frame with the local map composed of historical frames using a normal distribution transformation algorithm, thereby optimizing and correcting the global pose.

[0183] The third embodiment of the present invention provides a drawing device for engineering machinery, including a memory and a processor. The memory stores a computer program, which can be executed by the processor to implement a drawing method for engineering machinery as described in any of the above embodiments.

[0184] The fourth embodiment of the present invention provides a computer-readable storage medium, characterized in that it stores a computer program, which can be executed by a processor of the device in which the computer-readable storage medium is located, to implement a drawing method for engineering machinery as described in any of the above claims.

[0185] The key technical point of this invention, based on embodiments of the present invention, is the use of IMU data and IMU odometry to correct motion distortion in the current frame's laser point cloud. Point-line and point-surface features are extracted based on the curvature of each point in the current frame's laser point cloud. A local map is matched with the current frame's laser point cloud, and factor map optimization is performed on keyframes. The IMU zero bias is corrected based on the output laser odometry, the IMU pre-integrator is reset, and the output IMU odometry is used. When the current frame's laser point cloud meets the loop closure detection condition, a normal distribution transformation algorithm is used to perform loop closure detection on the current frame's laser point cloud and update the global pose. This solves the mapping problem in degraded scenarios for engineering machinery and achieves rapid mapping and localization in large scenes.

[0186] Exemplary examples show that the computer program described in the third and fourth embodiments of the present invention can be divided into one or more modules, which are stored in the memory and executed by the processor to complete the present invention. The one or more modules can be a series of computer program instruction segments capable of performing specific functions, which describe the execution process of the computer program in the mapping equipment for implementing engineering machinery. For example, the apparatus described in the second embodiment of the present invention.

[0187] The processor referred to can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or any conventional processor. The processor is the control center of the drawing method for engineering machinery, connecting various parts of the drawing method for engineering machinery through various interfaces and lines.

[0188] The memory can be used to store the computer program and / or modules. The processor, by running or executing the computer program and / or modules stored in the memory and calling the data stored in the memory, realizes various functions of a drawing method for engineering machinery. The memory may mainly include a program storage area and a data storage area. The program storage area may store the operating system, at least one application program required for a function (such as sound playback function, text conversion function, etc.), etc.; the data storage area may store data created according to the use of the mobile phone (such as audio data, text message data, etc.). In addition, the memory may include high-speed random access memory, and may also include non-volatile memory, such as hard disk, memory, plug-in hard disk, smart media card (SMC), secure digital (SD) card, flash card, at least one disk storage device, flash memory device, or other volatile solid-state storage device.

[0189] If the implemented module is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the above embodiments of the present invention can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content included in the computer-readable medium can be appropriately added or removed according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable media do not include electrical carrier signals and telecommunication signals.

[0190] It should be noted that the device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Furthermore, in the accompanying drawings of the device embodiments provided by this invention, the connection relationships between modules indicate that they have communication connections, which can be specifically implemented as one or more communication buses or signal lines. Those skilled in the art can understand and implement this without any creative effort.

[0191] The above description is merely a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A method for drawing engineering machinery, characterized in that, include: The environmental information collected by the vehicle-mounted multi-sensor fusion platform is acquired, and motion distortion correction is performed on the current frame laser point cloud of the environmental information using IMU data and IMU odometry. Based on the curvature of each point in the current frame's laser point cloud after correction, point-line features and point-surface features are extracted, and the point-line features and point-surface features are matched with the local map; Determine whether the current frame laser point cloud is a keyframe; If not, correct the IMU bias and reset the IMU pre-integrator; If so, factor map optimization is performed on the current frame's laser point cloud, and the current frame's laser point cloud is registered with the local map composed of historical frames using a normal distribution transformation algorithm to optimize and correct the global pose. Specifically: When the current frame of the laser point cloud is determined to be a key frame, the loop closure detection thread is invoked based on the laser odometry. The loop closure detection thread determines whether there are any historical keyframes in the historical keyframe set that are similar to the current keyframe. If so, determine whether the timestamp interval between the current keyframe and the nearby historical keyframe is greater than a threshold. If it is greater than the threshold, use the normal distribution transformation algorithm to match the current keyframe with the nearby historical keyframe to generate a relative coordinate transformation matrix, specifically: By rasterizing similar historical keyframes into target point clouds, and assuming that each grid conforms to a normal distribution, the probability density function of the laser point cloud distribution in each grid is calculated. The calculation model is as follows: in, Let the dimension be the vector. It is the mean vector. Let be the covariance matrix of a random vector; The target point cloud is placed into each grid, and the mean value of the laser point cloud data in each grid is calculated. The calculation model is as follows: in, The coordinates of all points in the point cloud within a single grid cell; The covariance of the point cloud data in each grid is calculated using the covariance calculation formula. The calculation model is as follows: The current keyframe is placed into each grid as the source point cloud for point cloud registration using the normal distribution transform algorithm. The probability of each point in the source point cloud in each grid is calculated, and the calculation model is as follows: The maximum likelihood function is obtained by multiplying the probabilities of each point in the source point cloud in each grid cell. When the maximum likelihood function reaches its maximum value, the relative coordinate transformation matrix is ​​the optimal solution. The calculation model is as follows: in, Let be the rotation matrix of the source point cloud relative to the target point cloud. Let be the translation matrix of the source point cloud relative to the target point cloud. Let be the coordinate transformation matrix of the source point cloud relative to the target point cloud. The coordinates of each point in the source point cloud are given in the coordinate system of the target point cloud. The coordinates of each point in the source point cloud in its own coordinate system; Taking the logarithm of both sides of the maximum likelihood function, its computational model is as follows: The maximum likelihood function is solved using Newton's iteration method. The coordinate transformation matrix of the source point cloud relative to the target point cloud is used as the iteration variable. The coordinate transformation matrix corresponding to the maximum likelihood function is solved by continuously updating the iteration variable along the gradient descent direction. The calculation model is as follows: in, The Hessian matrix is ​​the maximum likelihood function. Let be the gradient vector of the maximum likelihood function. This is the coordinate transformation matrix of the source point cloud relative to the target point cloud; The global pose is optimized and corrected using the relative coordinate transformation matrix.

2. The method for drawing engineering machinery according to claim 1, characterized in that, The process of acquiring environmental information collected by the vehicle-mounted multi-sensor fusion platform and using IMU data and IMU odometry to perform motion distortion correction on the current frame laser point cloud of the environmental information specifically involves: The IMU data is pre-integrated to obtain the coordinate transformation matrix of the lidar from the end time of the current frame laser point cloud scanning to the start time of the scanning. Motion distortion correction is performed on the current frame laser point cloud using the coordinate transformation matrix; wherein, the coordinate transformation matrix is: ； in, This is the coordinate transformation matrix of each point in the current frame of the laser point cloud relative to the initial scanning time. Let be the coordinate transformation matrix of the lidar from the end of the scan to the start of the scan. The measurement time of each point in the laser point cloud in the current frame within the scanning cycle. The scan start time, The end time of the scan. These are the coordinates of each point in the current frame of the laser point cloud in the lidar coordinate system at the start of the scan. This represents the coordinates of each point in the laser point cloud of the current frame in the lidar coordinate system at the current moment.

3. The method for drawing engineering machinery according to claim 1, characterized in that, The step involves extracting point-line and point-surface features based on the curvature of each point in the corrected current frame laser point cloud, and then matching these features with the local map. Specifically: The curvature calculation model is invoked to generate the curvature of each point in the laser point cloud of the current frame. The curvature calculation model is as follows: in, For laser point cloud The curvature of a point For the first The distance from the point to the origin of the lidar coordinate system. For the first The distances of the five laser points before and after the point from the origin of the lidar coordinate system; The laser point cloud of the current frame is subjected to feature extraction and division according to a preset curvature threshold. Points in the laser point cloud of the current frame whose curvature is greater than the curvature threshold are determined to be corner points, and those whose curvature is less than the curvature threshold are determined to be face points. Voxel filtering is performed on the corner points and face points to generate the feature point cloud of the current frame; The kd-tree is used to find the points in the local map that are closest to the corner features in the current frame feature point cloud and connect them to form a line. The distance from the corner point in the current frame feature point cloud to this line is then generated. The specific calculation model is as follows: ； in, for The first time in the feature point cloud of the current frame at time step The coordinates of each corner point in the world coordinate system and For the first The coordinates of each corner point in two world coordinate systems on the corresponding edge feature in the local map; For the polygon features in the current frame's feature point cloud, the kd-tree is used to find the five closest points in the local map, and the following plane equation is constructed: ； Where A, B, C, and D are the coefficients of the plane equation, and x, y, and z are the coordinates of the five points in the local map that are most similar to the feature point cloud features of the current frame. Substitute the coordinates of five points corresponding to the feature point cloud features of the current frame in the local map into the plane equation to obtain the coefficients of the plane equation. Then, calculate the distance from the feature point cloud features of the current frame to the feature plane of the local map based on the coefficients of the plane equation. The calculation model is as follows: in, These are the coordinates of the face points in the current frame's feature point cloud in the world coordinate system. Based on the distances between the corner points of the current frame laser point cloud and the corresponding edge features of the local map, and the distances between the surface points of the current frame laser point cloud and the corresponding planar features of the local map, the following residual equation is constructed: in, For the current frame feature point cloud, the first The coordinates of a point in the world coordinate system For the current frame feature point cloud, the first The coordinates of the point in the lidar coordinate system For the current frame feature point cloud, the first The coordinate transformation matrix for transforming a point from the lidar coordinate system to the world coordinate system. This is a local map in the world coordinate system. and These are the feature point clouds of the current frame, numbered... The rotation and translation matrices for transforming a point from the lidar coordinate system to the world coordinate system; Using the residual equation and the Levember algorithm, the following iterative equation is constructed: in, Let be the Jacobian matrix of the residual equation with respect to the iteration variables. For the diagonal update operator, It is the identity matrix. For the increment of the variable in a single iteration, The residual equation is... For the current frame feature point cloud, the first The coordinates of the point in the lidar coordinate system For the current frame feature point cloud, the first The coordinate transformation matrix for transforming a point from the lidar coordinate system to the world coordinate system; The sum of residuals from feature matching is calculated using the following model: ； in, For the current frame feature point cloud, the first The residual values ​​between each corner point and the corresponding edge features of the local map. For the current frame feature point cloud, the first The residual values ​​between a point and the corresponding planar features of the local map. and These represent the number of corner points and face points in the feature point cloud of the current frame, respectively. The diagonal update operator is updated based on the sum of residuals from feature matching. If the sum of residuals is too large, the diagonal operator is increased; otherwise, the diagonal operator is decreased.

4. The method for drawing engineering machinery according to claim 1, characterized in that, The process of correcting the IMU bias and resetting the IMU pre-integrator specifically involves: Define a pre-integrator for optimizing IMU zero bias and a pre-integrator for calculating IMU odometer readings, respectively. Acquire laser odometry data and update the factor graph. Determine if the factor tree in the updated factor graph exceeds a preset value. If it does, reconstruct the factor graph. Add the IMU raw data pre-integration factor, zero bias factor, and current frame laser odometry factor between the current frame laser point cloud and the previous frame laser point cloud to the updated factor graph; The pose and velocity estimated by the pre-integrator used to optimize the IMU zero bias are used as the estimated values ​​of the pose factor and velocity factor of the current frame. The zero bias optimized in the previous frame is used as the estimated value of the zero bias factor of the current frame. Factor graph optimization is performed to generate the optimized and updated velocity and zero bias. Using the optimized and updated zero bias, reset the pre-integrator used to optimize the IMU zero bias; The optimized and updated speed and zero bias are tested. If the test requirements are not met, the factor graph is reconstructed for optimization. The optimized and updated zero-bias resets the pre-integrator used to calculate the IMU odometry, and pre-integrates the raw IMU data after the current frame laser point cloud time to obtain the pre-integration result; Using the state of the laser point cloud at the current frame moment obtained by the factor graph optimization and the IMU zero bias, combined with the pre-integration result, the state of the laser point cloud after the current frame moment is estimated and output as a high-frequency odometry.

5. A mapping device for engineering machinery, characterized in that, include: The distortion correction unit is used to acquire environmental information collected by the vehicle-mounted multi-sensor fusion platform, and to perform motion distortion correction on the current frame laser point cloud of the environmental information using IMU data and IMU odometry. The map matching unit is used to extract point-line features and point-surface features based on the curvature of each point in the current frame laser point cloud after correction, and to match the point-line features and point-surface features with the local map; The judgment unit is used to determine whether the current frame laser point cloud is a key frame; The IMU calibration unit is used to calibrate the IMU zero bias and reset the IMU pre-integrator. The global optimization unit is used to perform factor map optimization on the current frame's laser point cloud, and to register the current frame's laser point cloud with the local map composed of historical frames using a normal distribution transformation algorithm, thereby optimizing and correcting the global pose. Specifically: When the current frame of the laser point cloud is determined to be a key frame, the loop closure detection thread is invoked based on the laser odometry. The loop closure detection thread determines whether there are any historical keyframes in the historical keyframe set that are similar to the current keyframe. If so, determine whether the timestamp interval between the current keyframe and the nearby historical keyframe is greater than a threshold. If it is greater than the threshold, use the normal distribution transformation algorithm to match the current keyframe with the nearby historical keyframe to generate a relative coordinate transformation matrix, specifically: By rasterizing similar historical keyframes into target point clouds, and assuming that each grid conforms to a normal distribution, the probability density function of the laser point cloud distribution in each grid is calculated. The calculation model is as follows: in, Let the dimension be the vector. It is the mean vector. Let be the covariance matrix of a random vector; The target point cloud is placed into each grid, and the mean value of the laser point cloud data in each grid is calculated. The calculation model is as follows: in, The coordinates of all points in the point cloud within a single grid cell; The covariance of the point cloud data in each grid is calculated using the covariance calculation formula. The calculation model is as follows: The current keyframe is placed into each grid as the source point cloud for point cloud registration using the normal distribution transform algorithm. The probability of each point in the source point cloud in each grid is calculated, and the calculation model is as follows: The maximum likelihood function is obtained by multiplying the probabilities of each point in the source point cloud in each grid cell. When the maximum likelihood function reaches its maximum value, the relative coordinate transformation matrix is ​​the optimal solution. The calculation model is as follows: in, Let be the rotation matrix of the source point cloud relative to the target point cloud. Let be the translation matrix of the source point cloud relative to the target point cloud. Let be the coordinate transformation matrix of the source point cloud relative to the target point cloud. The coordinates of each point in the source point cloud are given in the coordinate system of the target point cloud. The coordinates of each point in the source point cloud in its own coordinate system; Taking the logarithm of both sides of the maximum likelihood function, its computational model is as follows: The maximum likelihood function is solved using Newton's iteration method. The coordinate transformation matrix of the source point cloud relative to the target point cloud is used as the iteration variable. The coordinate transformation matrix corresponding to the maximum likelihood function is solved by continuously updating the iteration variable along the gradient descent direction. The calculation model is as follows: in, The Hessian matrix is ​​the maximum likelihood function. Let be the gradient vector of the maximum likelihood function. This is the coordinate transformation matrix of the source point cloud relative to the target point cloud; The global pose is optimized and corrected using the relative coordinate transformation matrix.

6. A mapping device for engineering machinery, characterized in that, It includes a memory and a processor, wherein the memory stores a computer program that can be executed by the processor to implement a drawing method for engineering machinery as described in any one of claims 1 to 4.

7. A computer-readable storage medium, characterized in that, The device contains a computer program that can be executed by a processor of the device on which the computer-readable storage medium is located, to implement a method for drawing engineering machinery as described in any one of claims 1 to 4.

Images