A multi-sensor fusion mapping method and device for degraded scenarios

Abstract

This invention discloses a multi-sensor fusion mapping method and apparatus for degraded scenarios, comprising: a front-end fusion odometry method: receiving multi-source data and obtaining initial pose through coordinate system alignment; performing point cloud distortion removal through IMU integration and extrinsic parameter transformation to generate a priori LiDAR pose; analyzing point pair contribution vectors based on ICP Hessian matrix decomposition to determine pose degradation; for non-degraded frames, constructing LiDAR point-to-surface residuals and RTK pose residuals, updating the state through ESIKF, and outputting keyframe pose, point cloud, and degradation labels; a back-end factor graph optimization method: filtering non-degraded keyframes through a sliding window, optimizing pose through point cloud beam adjustment; performing secondary verification of RTK pose to remove invalid values; constructing odometry, loop closure, and RTK factors, and fusing and optimizing to obtain a globally consistent high-precision point cloud map. This method enhances robustness through degradation detection and ensures global consistency through factor graph fusion, making it suitable for mapping complex scenarios.

Description

Technical Field

[0001] This invention relates to the field of intelligent driving technology, specifically to a multi-sensor fusion mapping method and apparatus for degraded scenarios. Background Technology

[0002] Achieving accurate and reliable positioning in various complex environments is a fundamental requirement for mobile robots to achieve autonomous navigation. In order to achieve stable and accurate positioning, it is necessary to construct a high-precision map of the current environment to perform map-based positioning.

[0003] Existing mapping methods mostly employ Simultaneous Localization and Mapping (SLAM) technology, which is mainly divided into LiDAR SLAM and Visual SLAM based on the different sensors used. LiDAR can provide the system with rich environmental structural information and is unaffected by changes in lighting, enabling high-precision positioning. However, its performance degrades significantly in degraded environments with few structural features. In contrast, visual cameras are less expensive and can capture color and texture information of the environment, but they are easily affected by changes in lighting and viewpoint, and cannot perceive depth information, requiring the construction of epipolar constraints for depth estimation. Therefore, their accuracy is generally lower than that of LiDAR SLAM. In addition, Inertial Measurement Units (IMUs) can provide accurate position information in the short term through integration, but the cumulative error gradually increases over time. Real-Time Kinematic (RTK) is a high-precision satellite navigation positioning technology that can achieve centimeter-level real-time positioning, but it is very susceptible to the influence of building occlusion, leading to a significant increase in error. Due to the limitations of a single sensor, multi-sensor-based multimodal SLAM technology has emerged, which can achieve a more accurate and robust SLAM system by compensating for the disadvantages of different sensors.

[0004] To construct high-precision maps for positioning, existing methods mostly employ SLAM (Simultaneous Localization and Mapping) that fuses LiDAR and IMU (Integrated Measurement Unit). This approach can ensure robustness in some weakly degraded scenarios. To further improve robustness and ensure global consistency of the map, some schemes integrate RTK (Real-Time Kinematics) as an absolute observation into the SLAM system. However, these methods are mostly added as factors in the backend of the system for optimization. Furthermore, the pose accuracy of RTK relies excessively on the covariance provided by the RTK itself. Therefore, in severely degraded scenarios such as airports, highway service areas, and open roads, map construction still fails and ghosting occurs. Thus, there is an urgent need for a highly robust and high-precision mapping method that can cope with these extreme scenarios. Summary of the Invention

[0005] The purpose of this invention is to provide a multi-sensor fusion mapping method for degraded scenarios, so as to solve the problems mentioned in the background art.

[0006] To achieve the above objectives, the present invention provides the following technical solution:

[0007] Firstly, a multi-sensor fusion mapping method for degraded scenarios includes a front-end fusion odometry stage and a back-end factor map optimization stage, wherein the front-end fusion odometry stage includes:

[0008] Receive data from lidar, IMU, and RTK, and align them with the coordinate system to obtain the initial pose;

[0009] Motion integration is performed using the acceleration and angular velocity data from the IMU to obtain the pose of each point in the LiDAR scan frame and the last point. By transforming the extrinsic parameters of the IMU and the LiDAR, all points in the scan frame are projected to the end of the frame, thus completing the point cloud distortion removal and obtaining the LiDAR prior pose.

[0010] Projecting the current point cloud onto the global coordinate system and matching it with the global map yields point pairs. The Hessian matrix of the ICP is decomposed to obtain 6 feature vectors. The contribution vectors and alignment values ​​of the point pairs are analyzed, "strong / weak alignment" is divided and invalid information is filtered out. The contribution vectors of strong localizability and combined localizability are summed to obtain the contribution vectors. The localizability of each principal direction is determined by combining the threshold. If any direction is unconstrained, it is defined as degeneracy.

[0011] The non-degenerate lidar point cloud is used to construct point-area distance residual constraints. The RTK data that meets the accuracy requirements is transformed and transformed by extrinsic parameters to construct pose-pose residual constraints. The state is updated based on ESIKF, and the key frame pose, point cloud, degenerate label and RTK pose are output.

[0012] The backend factor graph optimization stage includes:

[0013] Non-degenerate keyframes are filtered out using a sliding window in the front-end output, and their poses are adjusted and optimized using point cloud bundles.

[0014] Invalid values ​​are removed during the secondary verification of RTK pose.

[0015] By constructing a factor map using the optimized pose and RTK pose, a globally consistent high-precision point cloud map is obtained.

[0016] Preferably, in the front-end fusion odometer stage, the condition for determining that the RTK pose accuracy meets the requirements when the coordinate system is aligned is: the status bit field of the RTK data is in RTK stable solution localization and orientation mode and the pose covariance is less than 0.02-0.05 meters.

[0017] Preferably, in the front-end fusion odometer stage, the alignment value of the degradation detection is obtained by calculating the cosine of the angle between the contribution vector and the feature vector, wherein the absolute value of the alignment value of "strong alignment" is ≥ cos (45°), the absolute value of the alignment value of "weak alignment" is ≥ cos (60°) and < cos (45°), and information with an absolute value of alignment value < cos (60°) is filtered out.

[0018] Preferably, in the front-end fusion odometer stage, the determination of the localizability category of degradation detection includes preset thresholds such as a fully constrained threshold k1=250, an unconstrained threshold k3=35, and a partially constrained threshold k2 between k1 and k3; if the strong localizability contribution vector Ls≥k1, it is fully constrained, and if Ls<k3, it is unconstrained; otherwise, if Lc≥k2, it is partially constrained, and vice versa, it is unconstrained.

[0019] Preferably, in the front-end fusion odometer stage, the lidar point cloud residual is constructed in the following way: traversing the point cloud and projecting it onto the global map, searching for n nearest neighbor points based on KNN, calculating the covariance matrix and obtaining the plane normal vector through SVD decomposition, selecting the nearest point and calculating the point-plane distance residual using the vector dot product.

[0020] Preferably, in the back-end factor graph optimization stage, the adaptive voxelization of point cloud bundle adjustment includes: constructing a global point cloud, recursively dividing voxels, and determining whether points fall on the same edge / plane by calculating the eigenvalues ​​of the covariance matrix within the voxels to achieve feature association. If the condition is not met, the voxels are further divided.

[0021] Preferably, in the back-end factor graph optimization stage, the threshold for RTK secondary verification is 20%, that is, the RTK pose of key frames with a residual increment percentage greater than 20% is set to invalid.

[0022] Preferably, in the back-end factor graph optimization stage, the factor graph optimization is based on GTSAM to construct odometer factors, loop closure detection factors, and RTK location factors.

[0023] Secondly, the present invention provides a multi-sensor fusion mapping device for degraded scenarios, comprising a front-end module and a back-end module, wherein the front-end module includes:

[0024] The alignment module receives data from the lidar, IMU, and RTK, and aligns it with the coordinate system to obtain the initial pose;

[0025] The state estimation module uses the acceleration and angular velocity of the IMU data to perform motion integration, obtains the pose of each point in the LiDAR scan frame and the last point, and projects all points in the scan frame to the end of the frame through the extrinsic parameter transformation of the IMU and the LiDAR, thus completing the point cloud distortion removal and obtaining the LiDAR prior pose.

[0026] The degradation detection module projects the current point cloud onto the global coordinate system and matches it with the global map to obtain point pairs. It decomposes the Hessian matrix of the ICP to obtain 6 feature vectors, analyzes the contribution vector and alignment value of the point pairs, classifies them into "strong / weak alignment" and filters out invalid information, sums them to obtain the contribution vectors of strong localization and combined localization, and combines the threshold to determine the localization of each main direction. If any direction is unconstrained, it is defined as degradation.

[0027] The observation update module constructs point-area distance residual constraints from the non-degenerate lidar point cloud, constructs pose-pose residual constraints from the RTK data that meets the accuracy requirements through transformation and extrinsic parameter transformation, updates the state based on ESIKF, and outputs keyframe pose, point cloud, degenerate label and RTK pose.

[0028] The backend module includes:

[0029] The lidar point cloud beam adjustment module is used to filter non-degenerate keyframes through the front-end output sliding window and optimize the pose through point cloud beam adjustment.

[0030] The RTK secondary verification module removes invalid values ​​during the RTK pose secondary verification.

[0031] The factor graph optimization module uses the optimized pose and RTK pose to construct a factor graph, thereby obtaining a globally consistent high-precision point cloud map.

[0032] Thirdly, the present invention provides an electronic device including at least one processor, the processor being communicatively connected to at least one memory, wherein the memory stores instructions executable by the at least one processor, the instructions being executed by the at least one processor to enable the at least one processor to perform the method described above.

[0033] Compared with the prior art, the beneficial effects of the present invention are:

[0034] High robustness: By adding a high-frequency RTK global absolute pose observation and degradation detection module to the front-end odometer, localization and mapping can be performed by relying on RTK global absolute pose observation when the lidar point cloud observation is degraded, ensuring stable localization and mapping even in severely degraded scenarios.

[0035] High-precision mapping: In the backend factor map optimization stage, a set of high-precision keyframe poses is first obtained through the LiDAR point cloud beam adjustment module to improve the local consistency of the point cloud map and eliminate most ghosting. Then, the RTK secondary verification module filters out low-precision RTK global absolute pose observations caused by interference such as building occlusion, selecting high-precision RTK global absolute pose observations to improve the global consistency of the point cloud map. Finally, the high-precision keyframe poses and high-precision RTK global absolute poses are constructed as factors and added to the factor map for optimization, resulting in a high-precision map with good local and global consistency. Attached Figure Description

[0036] Figure 1 This is an overall flowchart of the present invention;

[0037] Figure 2 shows a partial technical solution diagram of the front-end lidar-IMU-RTK fusion odometry;

[0038] Figure 3 shows the technical solution for the backend factor graph optimization.

[0039] Figure 4 A schematic diagram of an electronic device according to an embodiment of the present invention. Detailed Implementation

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

[0041] Please see Figures 1 to 3 This invention provides a technical solution: a multi-sensor fusion mapping method for degraded scenarios, comprising a front-end fusion odometry stage and a back-end factor map optimization stage, wherein the front-end fusion odometry stage includes:

[0042] S100: Receives data from lidar, IMU, and RTK, and obtains the initial pose by aligning the coordinate system.

[0043] In embodiments of the present invention, by continuously receiving data from the lidar, IMU, and RTK, the pose accuracy of the RTK data is determined based on the state bit field and pose covariance. After receiving RTK data that meets the accuracy requirements for the first time, the corresponding coordinates are converted from the WGS84 coordinate system to the ENU coordinate system as the origin of the global coordinate system. Based on the extrinsic parameters between the RTK and the lidar, and between the RTK and the IMU, the initial poses of the lidar and the IMU in the global coordinate system are calculated.

[0044] In a specific embodiment of the present invention, during the front-end fusion odometer stage, the condition for determining that the RTK pose accuracy meets the requirements when aligning the coordinate system is: the status bit field of the RTK data is in RTK stable solution localization and orientation mode and the pose covariance is less than 0.02-0.05 meters. Preferably, it is 0.03 meters.

[0045] S200: Motion integration is performed using the acceleration and angular velocity data from the IMU to obtain the pose of each point in the LiDAR scan frame and the last point. By transforming the extrinsic parameters of the IMU and the LiDAR, all points in the scan frame are projected to the end of the frame, thus completing the point cloud distortion removal and obtaining the LiDAR prior pose.

[0046] In a specific embodiment of the present invention, step S200 specifically includes: first, taking the pose updated by the previous observation residual as the starting point, and then performing motion integration based on the acceleration and angular velocity provided by the received IMU data to obtain the pose of each point in the lidar scanning frame. pose of the last point in the lidar scan frame And by transforming the extrinsic parameters between the IMU and the lidar, all points in the current lidar scan frame are... Projecting onto the end of the lidar scan frame, we complete the lidar point cloud distortion correction and obtain the lidar prior pose for the current frame: in, Indicates the first i The original coordinates of each laser point in the laser sub-coordinate system L at the time of acquisition. This represents the noise of the laser point. This represents the pose transformation matrix from the laser coordinate system L to the IMU coordinate system I. Indicates the last laser frame Pose transformation to global coordinate system G. Indicates the first k The first frame p i At each laser point acquisition time, the IMU coordinate system ( I k-pi The pose transformation matrix from the global coordinate system (G) to the global coordinate system (G). Indicates after distortion correction, the first i The point in the current laser frame L k Coordinates in a coordinate system Current laser frame L k Pose transformation to the global coordinate system G.

[0047] S300: Project the current point cloud onto the global coordinate system and match it with the global map to obtain point pairs. Decompose the Hessian matrix of the ICP to obtain 6 feature vectors. Analyze the contribution vector and alignment value of the point pairs, classify them into "strong / weak alignment" and filter out invalid information. Summate to obtain the contribution vectors of strong localization and combined localization. Combine the threshold to determine the localization of each principal direction. If any direction is unconstrained, it is defined as degeneracy.

[0048] In an embodiment of the present invention, in the front-end fusion odometer stage, the alignment value of the degradation detection is obtained by calculating the cosine of the angle between the contribution vector and the feature vector, wherein the absolute value of the alignment value of "strong alignment" is ≥ cos (45°), the absolute value of the alignment value of "weak alignment" is ≥ cos (60°) and < cos (45°), and information with an absolute value of alignment value < cos (60°) is filtered out.

[0049] In an embodiment of the present invention, during the front-end fusion odometer stage, the determination of the localizability category of degradation detection includes preset thresholds such as a fully constrained threshold k1=250, an unconstrained threshold k3=35, and a partially constrained threshold k2 between k1 and k3. If the strong localizability contribution vector Ls≥k1, it is fully constrained; if Ls<k3, it is unconstrained; otherwise, if Lc≥k2, it is partially constrained, and vice versa.

[0050] In a specific embodiment of the present invention, after obtaining the prior pose of the LiDAR, the point cloud in the current LiDAR scan frame is first projected onto the global coordinate system and scan-matched with the global map to obtain matching point pairs. Each point pair contains a point in the global map, the corresponding point in the current LiDAR scan frame, and its surface normal. Then, by performing eigenvalue decomposition on the Hessian matrix in the Iterative Closest Point (ICP) algorithm optimization problem, six eigenvectors are obtained. Each eigenvector represents a principal direction of the optimization problem, with three corresponding to translation and three corresponding to rotation. Based on this, information analysis is performed, including three core steps: coordinate system transformation, calculation of "torque," and calculation of alignment value. The coordinate system transformation converts all matching point pairs from the global coordinate system back to the LiDAR sensor coordinate system, thereby eliminating the influence of the map's physical size on the rotation direction positioning analysis, making the detection result independent of the sensor's orientation in the map. For each matching point pair, the "torque" is calculated, which is the distance from the point to the corresponding surface normal, similar to the concept of torque. The resulting value represents the contribution vector τ. Then, the alignment value is calculated, which is the degree of alignment between each contribution vector τ and the currently analyzed feature vector. This is achieved by calculating the cosine of the angle between them. Finally, all matching point pairs and their contribution vectors and alignment values ​​to each feature vector are obtained. Next, the alignment degree is obtained by comparing the alignment value with a threshold. The contribution information is divided into "strong alignment" and "weak alignment," and useless information is filtered out. If the absolute value of the alignment value of a point pair is greater than or equal to cos(45°) ≈ 0.707, the point pair is considered to have provided a strong contribution, and its contribution vector is retained. If the absolute value of the alignment value of a point pair is greater than or equal to cos(60°) = 0.5 but less than cos(45°) ≈ 0.707, the point pair is considered to have provided a weak but potentially reliable contribution, and its contribution vector is also retained. If the absolute value of the alignment value of a point pair is less than cos(60°) = 0.5, then the contribution direction of that point pair is considered to have too weak a relationship with the current main direction of analysis and cannot provide effective constraints, and is therefore discarded. The contribution vectors classified as "strongly aligned" are summed to obtain the strong locatability contribution vector Ls, reflecting the amount of high-quality information in the current main direction. The contribution vectors classified as "strongly aligned" and "weakly aligned" are summed to obtain the combined locatability contribution vector Lc, reflecting the total amount of all potentially useful information in the current main direction; these vectors quantify the information intensity in the characteristic direction of the current analysis. Finally, based on the values ​​of the strong locatability contribution vector Ls and the combined locatability contribution vector Lc, combined with preset thresholds, a locatability category is assigned to each main direction: fully constrained, partially constrained, or unconstrained. The three preset counting thresholds are:

[0051] k1: The threshold for complete constraint, which is 250;

[0052] k2: The threshold for partial constraint, which is between k1 and k3;

[0053] k3: The threshold for non - constraint, which is 35;

[0054] Then, a decision tree based on Ls and Lc is performed to independently judge each feature vector. First, check whether it is "fully constraint". If Ls is greater than or equal to k1, it is considered that there is sufficient strong information and it is judged as fully constraint. Then, check whether it is "non - constraint". If Ls < k3, it is considered that there is serious lack of even the most basic strong information and it is judged as non - constraint. If neither of the above two conditions is satisfied, enter the branch of checking whether it is "partially constraint". At this time, check the value of Lc again. If Lc >= k2, it is considered that although there is not much strong information, the total amount of combined weak information is acceptable, and there are sparse but valuable constraints, and it is judged as partially constraint. If Lc < k2, it is considered that the total amount of information is also too low and it is judged as non - constraint. If any one of the main directions is non - constraint, the pose of this key frame is defined as degraded.

[0055] S400: Construct point - plane distance residuals for non - degraded lidar point clouds, construct pose - pose residuals for RTK data that meet the accuracy after conversion and extrinsic parameter transformation, update the state based on ESIKF, and output the key frame pose, point cloud, degradation label, and RTK pose.

[0056] In the embodiment of the present invention, in the front - end fusion odometer stage, the lidar point cloud residuals are constructed as follows: Traverse the point cloud projected onto the global map, search for n nearest neighbor points based on KNN, calculate the covariance matrix and perform SVD decomposition to obtain the plane normal vector, and select the nearest point to calculate the point - plane distance residual using the vector dot product.

[0057] In a specific embodiment of the present invention, after obtaining the lidar prior pose, motion observations are provided by two observation sources, lidar and RTK, for state update. For the point cloud data observations provided by the lidar, after passing through the degradation detection module, the non - degraded lidar scan frames are used to construct point - plane distance residuals. The specific operation is to traverse the points in the current lidar scan frame , and project them into the global map: Search for n nearest neighbor points of the current point in the map based on KNN search, calculate the covariance matrix of these n points, perform eigenvalue decomposition using SVD, and calculate the plane normal vector according to the eigenvalues and eigenvectors , and select the point nearest to the The point-to-surface distance residual is calculated using vector dot product. For global absolute pose observations provided by RTK, RTK pose accuracy is judged based on the state bit field and pose covariance of the RTK data. Only data with the state bit field in the RTK stable solution localization and orientation mode and a pose covariance less than 0.03 meters are judged to meet the pose accuracy requirements. After receiving RTK data that meets the pose accuracy requirements, the corresponding coordinates are converted from the WGS84 coordinate system to the ENU coordinate system. Then, the global absolute pose of the IMU in the global coordinate system is obtained through extrinsic parameter transformation between RTK and IMU, thus constructing pose-pose residual constraints. After obtaining the two types of observations, the state is updated based on the Error-State Iterative Kalman Filter (ESIKF).

[0058] In an embodiment of the present invention, the backend factor graph optimization stage includes the following steps:

[0059] S500: The front-end output slide window filters non-degenerate keyframes, and the pose is adjusted and optimized by point cloud bundles.

[0060] In an embodiment of the present invention, the adaptive voxelization of point cloud bundle adjustment in the back-end factor graph optimization stage includes: constructing a global point cloud, recursively dividing voxels, and determining whether points fall on the same edge / plane by calculating the eigenvalues ​​of the covariance matrix within the voxels to achieve feature association. If the condition is not met, the voxels are further divided.

[0061] In a specific embodiment of the present invention, this step is as follows: The keyframe poses, keyframe point clouds, and degradation detection labels output by the front-end odometer are read. First, the data is divided into multiple windows using a sliding window mechanism. Then, for the keyframe poses and keyframe point clouds within each window, degraded keyframe point clouds and their corresponding poses are selected based on the degradation detection labels and are not included in subsequent optimization. The remaining keyframe point clouds undergo feature extraction and feature association through adaptive voxelization. Adaptive voxelization includes three core steps: global point cloud construction, recursive voxel partitioning, and feature judgment and association. The global point cloud construction part uses the keyframe poses and keyframe point clouds output by the front-end odometer to transform all scanned points into a global coordinate system, forming a preliminary global point cloud. The recursive voxel partitioning part divides the entire space into a large voxel and then recursively subdivides the voxel into smaller sub-voxels. In the feature judgment and association part, for each sub-voxel, the eigenvalues ​​of the in-voxel covariance matrix are calculated to determine whether the points within the voxel fall on the same edge / plane. If so, the current voxel is retained and the points within it are associated with this geometric feature; otherwise, the voxel is divided into 8 smaller voxels, and the above operation is repeated. Then, a point clustering method is used to aggregate all points associated with the same feature into a "point cluster." This cluster is represented by a compact set of parameters, thus eliminating the need to store and process each original point. After obtaining all point clusters, a cost function is constructed by minimizing the sum of squared Euclidean distances from all points to their corresponding features. The analytical solution and fixation of the feature parameters ensure that the cost function only explicitly depends on the keyframe pose T of the LiDAR. When the pose T changes, the position of the point in the global coordinate system changes, resulting in a change in the distance from the point to the geometric feature. Finally, an iterative optimization solution based on a second-order Levenberg-Marquardt (LM) optimization solver is used to iteratively update the pose until convergence, resulting in a set of high-precision keyframe poses.

[0062] S600: Eliminates invalid values ​​during secondary verification of RTK pose.

[0063] In an embodiment of the present invention, during the back-end factor graph optimization stage, the threshold for RTK secondary verification is 20%, that is, the RTK pose of key frames with a residual increment percentage greater than 20% is set to invalid.

[0064] For each window divided in the LiDAR point cloud beam adjustment module, the corresponding keyframe RTK pose output by the front-end odometry is read. Analogous to the operation of the LiDAR point cloud beam adjustment module, a cost function is constructed, and the LiDAR keyframe pose is replaced with the corresponding keyframe RTK pose. Without iterative optimization, the residual value under the keyframe RTK pose is calculated. Then, the percentage increase of the residual value compared with the keyframe pose after iterative convergence is calculated. If the percentage increase of the residual value compared with the keyframe pose after iterative convergence is greater than 20%, it is considered that the accuracy of the keyframe RTK pose in this window is low due to building occlusion. The keyframe RTK pose in this window is set as an invalid RTK pose and will not participate in subsequent factor graph optimization.

[0065] S700: Construct a factor map using the optimized pose and RTK pose to obtain a globally consistent high-precision point cloud map.

[0066] In an embodiment of the present invention, during the back-end factor graph optimization stage, factor graph optimization is based on GTSAM to construct odometer factors, loop closure detection factors, and RTK location factors.

[0067] After passing through the lidar point cloud beam adjustment module and the RTK secondary verification module, optimized high-precision keyframe poses and high-precision keyframe RTK poses are obtained. For the optimized high-precision keyframe poses, odometry factors and loop closure detection factors in GTSAM are constructed using them. For the high-precision keyframe RTK poses, RTK position factors are constructed using them, and both are uniformly optimized through a factor map to obtain globally consistent high-precision keyframe poses and corresponding global point cloud maps.

[0068] Furthermore, embodiments of the present invention also provide a multi-sensor fusion mapping device for degraded scenarios, used to implement the method of any of the above embodiments. The device includes a front-end module and a back-end module, wherein the front-end module includes:

[0069] Alignment module 100 receives data from lidar, IMU, and RTK, and obtains the initial pose by aligning the coordinate system.

[0070] The state estimation module 200 uses the acceleration and angular velocity of the IMU data to perform motion integration, obtains the pose of each point and the last point of the LiDAR scanning frame, and projects all points of the scanning frame to the end of the frame through the extrinsic parameter transformation of the IMU and the LiDAR, thereby completing the point cloud distortion removal and obtaining the LiDAR prior pose.

[0071] The degradation detection module 300 projects the current point cloud onto the global coordinate system and matches it with the global map to obtain point pairs. It decomposes the Hessian matrix of the ICP to obtain 6 feature vectors, analyzes the contribution vector and alignment value of the point pairs, classifies them into "strong / weak alignment" and filters out invalid information, sums them to obtain the contribution vectors of strong localizability and combined localizability, and determines the localizability of each main direction based on the threshold. If any direction is unconstrained, it is defined as degradation.

[0072] The observation update module 400 constructs point-to-area distance residual constraints from the non-degenerate lidar point cloud, and constructs pose-pose residual constraints from the RTK data that meets the accuracy requirements after transformation and extrinsic parameter transformation. Based on ESIKF, the state is updated, and the keyframe pose, point cloud, degenerate label and RTK pose are output.

[0073] The backend module includes:

[0074] The lidar point cloud beam adjustment module 500 is used to filter non-degenerate keyframes through the front-end output sliding window and optimize the pose through point cloud beam adjustment.

[0075] The RTK secondary verification module 600 removes invalid values ​​during the RTK pose secondary verification.

[0076] The factor graph optimization module 700 uses the optimized pose and RTK pose to construct a factor graph, thereby obtaining a globally consistent high-precision point cloud map.

[0077] In a specific embodiment of the present invention, the working principle of the initial coordinate system alignment module 100 is as follows: continuously receiving LiDAR, IMU and RTK data, and judging the RTK pose accuracy based on the state bit field and pose covariance of the RTK data. After receiving RTK data that meets the pose accuracy requirements for the first time, the corresponding coordinates are converted from the WGS84 coordinate system to the ENU coordinate system as the origin of the global coordinate system, and the initial pose of the LiDAR and IMU in the global coordinate system is obtained based on the extrinsic parameters between RTK and LiDAR and between RTK and IMU.

[0078] In a specific embodiment of the present invention, the working principle of the state estimation module 200 is as follows: First, the pose updated by the previous observation residual is used as the starting point. Then, motion integration is performed based on the acceleration and angular velocity provided by the received IMU data to obtain the pose of each point in the lidar scanning frame and the pose of the last point in the lidar scanning frame. Then, all points in the current lidar scanning frame are projected to the end of the lidar scanning frame through the extrinsic parameter transformation between the IMU and the lidar, thereby completing the lidar point cloud distortion removal and obtaining the lidar prior pose of the current frame.

[0079] In a specific embodiment of the present invention, the degradation detection module 300 works as follows: After obtaining the prior pose of the lidar, the point cloud in the current lidar scan frame is first projected onto the global coordinate system and scanned and matched with the global map to obtain matching point pairs. Then, by performing eigenvalue decomposition on the Hessian matrix in the Iterative Closest Point (ICP) algorithm optimization problem, six eigenvectors are obtained. Each eigenvector represents a principal direction of the optimization problem, with three corresponding to translation and three to rotation. Based on this, information analysis is performed to obtain all matching point pairs and their contribution vectors and alignment values ​​to each eigenvector. Next, the alignment degree is obtained by comparing the alignment value with a threshold. The contribution information is divided into "strong alignment" and "weak alignment," and useless information is filtered out. All contribution vectors classified as "strong alignment" are summed to obtain a strong localizability contribution vector, reflecting the amount of high-quality information in the current principal direction. All contribution vectors classified as "strong alignment" and "weak alignment" are summed to obtain a combined localizability contribution vector, reflecting the total amount of all potentially useful information in the current principal direction. Finally, based on the values ​​of the strong localizability contribution vector and the combined localizability contribution vector, combined with a preset threshold, a localizability category is assigned to each principal direction: fully constrained, partially constrained, or unconstrained. If any principal direction is unconstrained, the keyframe pose is defined as degenerate.

[0080] In a specific embodiment of the present invention, the observation update module 400 works as follows: After obtaining the prior pose of the lidar, motion observations are provided by both lidar and RTK observation sources for state update. For point cloud data observations provided by lidar, after passing through the degradation detection module, the non-degradable lidar scan frames are used to construct point-to-surface distance residual constraints. For global absolute pose observations provided by RTK, RTK pose accuracy is judged based on the state bit field and pose covariance of the RTK data. After receiving RTK data that meets the pose accuracy requirements, the corresponding coordinates are converted from the WGS84 coordinate system to the ENU coordinate system. Then, the global absolute pose of the IMU in the global coordinate system is obtained through the extrinsic parameter transformation between RTK and IMU, thereby constructing pose-pose residual constraints. After obtaining the two observations, state update is performed based on the Error-State Iterative Kalman Filter (ESIKF).

[0081] In a specific embodiment of the present invention, the working principle of the lidar point cloud beam adjustment module 500 is as follows: It reads the keyframe pose, keyframe point cloud, and degradation detection label output by the front-end odometer. First, it divides the data into multiple windows using a sliding window. Then, for the keyframe pose and keyframe point cloud within each window, it filters out the degraded keyframe point cloud and its corresponding pose based on the degradation detection label, and these degraded points do not participate in subsequent optimization. The remaining keyframe point clouds undergo feature extraction and feature association through adaptive voxelization. A point clustering method is used to aggregate all points associated with the same feature into a single "point cluster," represented by a compact set of parameters. After obtaining all point clusters, a cost function is constructed by minimizing the sum of squared Euclidean distances from all points to their corresponding features. The cost function is made to explicitly depend only on the lidar keyframe pose by using analytical solutions and fixed feature parameters. Finally, an iterative optimization solution is performed using a second-order optimization solver based on Levenberg-Marquardt (LM), iteratively updating the pose until convergence, resulting in a set of high-precision keyframe poses.

[0082] In a specific embodiment of the present invention, the working principle of the RTK secondary verification module 600 is as follows: for each window divided in the lidar point cloud beam adjustment module, the corresponding key frame RTK pose output by the front-end odometer is read, the cost function is constructed in analogy with the operation of the lidar point cloud beam adjustment module, and the key frame pose of the lidar is replaced with the corresponding key frame RTK pose. Without iterative optimization, the residual value under the key frame RTK pose is calculated, and then the incremental percentage of the residual value under the key frame pose after iterative convergence is calculated. Key frame RTK poses that are greater than the threshold are set as invalid RTK poses and do not participate in subsequent factor graph optimization.

[0083] In a specific embodiment of the present invention, the working principle of the factor graph optimization module 700 is as follows: after passing through the lidar point cloud beam adjustment module and the RTK secondary verification module, optimized high-precision keyframe pose and high-precision keyframe RTK pose are obtained. For the optimized high-precision keyframe pose, it is used to construct the odometry factor and loop closure detection factor in GTSAM; for the high-precision keyframe RTK pose, it is used to construct the RTK position factor, and both are uniformly optimized through the factor graph to obtain a globally consistent high-precision keyframe pose and a corresponding global point cloud map.

[0084] Please see Figure 4 , Figure 4 A schematic diagram of the mechanism of an electronic device 20 that can implement embodiments of the present invention is shown. The electronic device is intended to represent various forms of control devices. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the invention described and / or claimed herein.

[0085] Electronic device 20 includes at least one processor 21 and a memory, such as read-only memory (ROM) 22 and random access memory (RAM) 23, communicatively connected to at least one processor 21. The memory stores computer programs executable by at least one processor. The processor 11 can perform various appropriate actions and processes based on the computer program stored in the ROM 22 or loaded from storage unit 28 into the RAM 13. The RAM 23 may also store various programs and data required for the operation of electronic device 20. The processor 21, ROM 22, and RAM 23 are interconnected via bus 24. Input / output (I / O) interface 25 is also connected to bus 24.

[0086] Multiple components in electronic device 20 are connected to I / O interface 25, including: input unit 26, such as keyboard, mouse, etc.; output unit 27, such as various types of monitors, speakers, etc.; storage unit 18, such as disk, optical disk, etc.; and communication unit 29, such as network card, modem, wireless transceiver, etc. Communication unit 29 allows electronic device 20 to exchange information / data with other devices through computer networks such as the Internet and / or various telecommunications networks.

[0087] Processor 21 can be a variety of general-purpose and / or special-purpose processing components with processing and computing capabilities. Some examples of processor 21 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various special-purpose artificial intelligence (AI) computing chips, various processors running machine learning model algorithms, a digital signal processor (DSP), and any suitable processor, controller, microcontroller, etc. Processor 21 performs the various methods and processes described above.

[0088] In some embodiments, the methods described above can be implemented as a computer program tangibly contained in a computer-readable storage medium, such as storage unit 28. In some embodiments, part or all of the computer program can be loaded and / or installed on electronic device 20 via ROM 22 and / or communication unit 29. When the computer program is loaded into RAM 23 and executed by processor 21, one or more steps of the methods described above can be performed. Alternatively, in other embodiments, processor 21 can be configured to perform the methods described above by any other suitable means (e.g., by means of firmware).

[0089] Various embodiments of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems-on-a-chip (SoCs), payload-programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various embodiments may include implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transmitting data and instructions to the storage system, the at least one input device, and the at least one output device.

[0090] Computer programs used to implement the methods of the present invention may be written in any combination of one or more programming languages. These computer programs may be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, such that when executed by the processor, the computer programs cause the functions / operations specified in the flowcharts and / or block diagrams to be performed. The computer programs may be executed entirely on a machine, partially on a machine, or as a standalone software package, partially on a machine and partially on a remote machine, or entirely on a remote machine or server.

[0091] In the context of this invention, a computer-readable storage medium can be a tangible medium that may contain or store a computer program for use by or in conjunction with an instruction execution system, apparatus, or device. A computer-readable storage medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination thereof. Alternatively, a computer-readable storage medium may be a machine-readable signal medium. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0092] To provide interaction with a user, the systems and techniques described herein can be implemented on an electronic device having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user; and a keyboard and pointing device (e.g., a mouse or trackball) through which the user provides input to the electronic device. Other types of devices can also be used to provide interaction with the user; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including sound input, voice input, or tactile input).

[0093] The systems and technologies described herein can be implemented in computing systems that include backend components (e.g., as data servers), or computing systems that include middleware components (e.g., application servers), or computing systems that include frontend components (e.g., user computers with graphical user interfaces or web browsers through which users can interact with implementations of the systems and technologies described herein), or any combination of such backend, middleware, or frontend components. The components of the system can be interconnected via digital data communication of any form or medium (e.g., communication networks). Examples of communication networks include local area networks (LANs), wide area networks (WANs), blockchain networks, and the Internet.

[0094] A computing system can include clients and servers. Clients and servers are generally located far apart and typically interact through communication networks. Client-server relationships are created by computer programs running on the respective computers and having a client-server relationship with each other. The server can be a cloud server, also known as a cloud computing server or cloud host, which is a hosting product within the cloud computing service system to address the shortcomings of traditional physical hosts and VPS services, such as high management difficulty and weak business scalability.

[0095] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A multi-sensor fusion mapping method for degenerative scenarios, characterized in that, The method comprises a front-end fusion odometry stage and a back-end factor graph optimization stage, wherein the front-end fusion odometry stage comprises: Receiving lidar, IMU and RTK data, and obtaining an initial pose through coordinate system alignment; Performing motion integration on the acceleration and angular velocity of the IMU data to obtain the pose of each point and the last point of a laser radar scanning frame, projecting all points of the scanning frame to the end of the frame through the external parameter transformation of the IMU and the laser radar, completing point cloud distortion removal and obtaining a laser radar prior pose; Projecting the current point cloud to the global coordinate system and the global map to obtain a point pair, decomposing the Hessian matrix of ICP to obtain six eigenvectors, analyzing the contribution vector and alignment value of the point pair, dividing "strong / weak alignment", filtering invalid information, summing to obtain a strong localizability and a combined localizability contribution vector, combining a threshold to determine the localizability of each main direction, and defining a degeneration if any direction is not constrained; Constructing a point-plane distance residual constraint for the non-degenerated laser radar point cloud, converting and externally transforming the RTK data satisfying the accuracy to construct a pose-pose residual constraint, updating the state based on ESIKF, and outputting the key frame pose, point cloud, degeneration label and RTK pose; The back-end factor graph optimization stage comprises: Screening non-degenerated key frames through a sliding window for the front-end output, and optimizing the pose through point cloud beam adjustment; Secondarily checking the RTK pose to remove invalid values; Constructing a factor graph using the optimized pose and the RTK pose, and optimizing to obtain a globally consistent high-precision point cloud map.

2. The multi-sensor fusion mapping method for degenerative scenes according to claim 1, wherein: In the front-end fusion odometry stage, the condition for judging that the RTK pose accuracy meets the requirements during coordinate system alignment is that the state bit field of the RTK data is in an RTK stable solution positioning and orientation mode and the pose covariance is less than 0.02-0.05 meters.

3. The multi-sensor fusion mapping method for degenerative scene according to claim 1, characterized in that: In the front-end fusion odometry stage, the alignment value of the degeneration detection is obtained by calculating the cosine value of the angle between the contribution vector and the eigenvector, wherein the absolute value of the alignment value of "strong alignment" is greater than or equal to cos (45°), the absolute value of the alignment value of "weak alignment" is greater than or equal to cos (60°) and less than cos (45°), and the information with an absolute value of the alignment value less than cos (60°) is filtered.

4. The multi-sensor fusion mapping method for degenerative scenes according to claim 3, characterized in that: In the front-end fusion odometry stage, in the localizability category determination of the degeneration detection, the preset threshold includes a complete constraint threshold k1=250, an unconstrained threshold k3=35, and a partial constraint threshold k2 between k1 and k3; if the strong localizability contribution vector Ls is greater than or equal to k1, it is completely constrained; if Ls is less than k3, it is unconstrained; otherwise, if the combined localizability contribution vector Lc is greater than or equal to k2, it is partially constrained; otherwise, it is unconstrained.

5. The multi-sensor fusion mapping method for degenerative scene according to claim 4, characterized in that: In the front-end fusion odometry stage, the laser radar point cloud residual is constructed by the following method: traversing the point cloud projection to the global map, searching for n nearest neighbor points based on KNN, calculating the covariance matrix and performing SVD decomposition to obtain a plane normal vector, and selecting the nearest point to calculate the point-plane distance residual through vector dot product.

6. The multi-sensor fusion mapping method for degenerative scene according to claim 5, characterized in that: In the backend factor graph optimization stage, adaptive voxelization of the point cloud beam adjustment includes: constructing a global point cloud, recursively dividing voxels, and judging whether a point falls on the same edge / plane by calculating eigenvalues of a covariance matrix in the voxel to achieve feature association, and if not, continue to divide the voxel.

7. The multi-sensor fusion mapping method for degenerative scene according to claim 6, characterized in that: In the backend factor graph optimization stage, the threshold for RTK secondary verification is 20%, that is, the key frame RTK pose whose residual increment percentage is greater than 20% is set as invalid.

8. The multi-sensor fusion mapping method for degenerative scene according to claim 7, characterized in that: In the backend factor graph optimization stage, factor graph optimization is based on GTSAM to construct odometry factors, loop detection factors and RTK position factors.

9. A multi-sensor fusion mapping device for a degenerative scenario, characterized in that, The method comprises a front-end module and a back-end module, wherein the front-end module comprises: An alignment module (100) receives lidar, IMU and RTK data, and obtains an initial pose through coordinate system alignment; A state estimation module (200) performs motion integration using acceleration and angular velocity of the IMU data to obtain the pose of each point and the last point of a laser radar scan frame, and projects all points of the scan frame to the end of the frame through the transformation of the IMU and the laser radar external parameters, to complete point cloud distortion removal and obtain a laser radar prior pose; A degeneration detection module (300) projects the current point cloud to the global coordinate system and matches the points with the global map to obtain a point pair, decomposes the Hessian matrix of ICP to obtain 6 characteristic vectors, analyzes the contribution vectors and alignment values of the point pair, divides "strong / weak alignment" and filters invalid information, sums to obtain strong and combined localizability contribution vectors, and determines the localizability of each main direction in combination with a threshold, and defines it as degeneration if any direction is not constrained; An observation update module (400) constructs a point-surface distance residual constraint for non-degenerate laser radar point cloud, converts and transforms RTK data to construct a pose-pose residual constraint, updates the state based on ESIKF, and outputs key frame pose, point cloud, degeneration label and RTK pose; The back-end module comprises: A lidar point cloud beam adjustment module (500) is configured to filter non-degenerate key frames from the front-end output sliding window, and optimize the pose through point cloud beam adjustment; An RTK secondary verification module (600) is configured to perform secondary verification on the RTK pose to remove invalid values; A factor graph optimization module (700) is configured to construct a factor graph using the optimized pose and RTK pose, and optimize to obtain a globally consistent high-precision point cloud map.

10. An electronic device, comprising: The method comprises a front-end module and a back-end module, wherein the front-end module comprises: An alignment module (100) receives lidar, IMU and RTK data, and obtains an initial pose through coordinate system alignment; A state estimation module (200) performs motion integration using acceleration and angular velocity of the IMU data to obtain the pose of each point and the last point of a laser radar scan frame, and projects all points of the scan frame to the end of the frame through the transformation of the IMU and the laser radar external parameters, to complete point cloud distortion removal and obtain a laser radar prior pose; A degeneration detection module (300) projects the current point cloud to the global coordinate system and matches the points with the global map to obtain a point pair, decomposes the Hessian matrix of ICP to obtain 6 characteristic vectors, analyzes the contribution vectors and alignment values of the point pair, divides "strong / weak alignment" and filters invalid information, sums to obtain strong and combined localizability contribution vectors, and determines the localizability of each main direction in combination with a threshold, and defines it as degeneration if any direction is not constrained; An observation update module (400) constructs a point-surface distance residual constraint for non-degenerate laser radar point cloud, converts and transforms RTK data to construct a pose-pose residual constraint, updates the state based on ESIKF, and outputs key frame pose, point cloud, degeneration label and RTK pose; The back-end module comprises: A lidar point cloud beam adjustment module (500) is configured to filter non-degenerate key frames from the front-end output sliding window, and optimize the pose through point cloud beam adjustment; An RTK secondary verification module (600) is configured to perform secondary verification on the RTK pose to remove invalid values; A factor graph optimization module (700) is configured to construct a factor graph using the optimized pose and RTK pose, and optimize to obtain a globally consistent high-precision point cloud map. The method comprises a front-end module and a back-end module, wherein the front-end module comprises: An alignment module (100) receives lidar, IMU and RTK data, and obtains an initial pose through coordinate system alignment; A state estimation module (200) performs motion integration using acceleration and angular velocity of the IMU data to obtain the pose of each point and the last point of a laser radar scan frame, and projects all points of the scan frame to the end of the frame through the transformation of the IMU and the laser radar external parameters, to complete point cloud distortion removal and obtain a laser radar prior pose; A degeneration detection module (300) projects the current point cloud to the global coordinate system and matches the points with the global map to obtain a point pair, decomposes the Hessian matrix of ICP to obtain 6 characteristic vectors, analyzes the contribution vectors and alignment values of the point pair, divides "strong / weak alignment" and filters invalid information, sums to obtain strong and combined localizability contribution vectors, and determines the localizability of each main direction in combination with a threshold, and defines it as degeneration if any direction is not constrained; An observation update module (400) constructs a point-surface distance residual constraint for non-degenerate laser radar point cloud, converts and transforms RTK data to construct a pose-pose residual constraint, updates the state based on ESIKF, and outputs key frame pose, point cloud, degeneration label and RTK pose; The back-end module comprises: A lidar point cloud beam adjustment module (500) is configured to filter non-degenerate key frames from the front-end output sliding window, and optimize the pose through point cloud beam adjustment; An RTK secondary verification module (600) is configured to perform secondary verification on the RTK pose to remove invalid values; A factor graph optimization module (700) is configured to construct a factor graph using the optimized pose and RTK pose, and optimize to obtain a globally consistent high-precision point cloud map. The method comprises a front-end module and a back-end module, wherein the front-end module comprises: An alignment module (100) receives lidar, IMU and RTK data, and obtains an initial pose through coordinate system alignment; A state estimation module (200) performs motion integration using acceleration and angular velocity of the IMU data to obtain the pose of each point and the last point of a laser radar scan frame, and projects all points of the scan frame to the end of the frame through the transformation of the IMU and the laser radar external parameters, to complete point cloud distortion removal and obtain a laser radar prior pose; A degeneration detection module (300) projects the current point cloud to the global coordinate system and matches the points with the global map to obtain a point pair, decomposes the Hessian matrix of ICP to obtain 6 characteristic vectors, analyzes the contribution vectors and alignment values of the point pair, divides "strong / weak alignment" and filters invalid information, sums to obtain strong and combined localizability contribution vectors, and determines the localizability of each main direction in combination with a threshold, and defines it as degeneration if any direction is not constrained; An observation update module (400) constructs a point-surface distance residual constraint for non-degenerate laser radar point cloud, converts and transforms RTK data to construct a pose-pose residual constraint, updates the state based on ESIKF, and outputs key frame pose, point cloud, degeneration label and RTK pose; The back-end module comprises: A lidar point cloud beam adjustment module (500) is configured to filter non-degenerate key frames from the front-end output sliding window, and optimize the pose through point cloud beam adjustment; An RTK secondary verification module (600) is configured to perform secondary verification on the RTK pose to remove invalid values; A factor graph optimization module (700) is configured to construct a factor graph using the optimized pose and RTK pose, and optimize to obtain a globally consistent high-precision point cloud map.

Images