Laser inertial navigation wheel speed fusion positioning and mapping method and system under dynamic degradation environment

Abstract

The application discloses a laser inertial navigation wheel speed fusion positioning and mapping method and system under a dynamic degradation environment, and belongs to the technical field of mobile robot positioning and mapping. The method synchronously collects laser radar, inertial measurement unit and wheel encoder data, pre-integrates the IMU and the wheel speed, removes dynamic points based on a three-dimensional voxel grid, Bayesian occupancy probability and time decay, removes point cloud distortion by using the pre-integration result, adaptively switches or weightedly fuses feature matching and direct residual registration according to environmental feature richness, and outputs a pose trajectory and a three-dimensional map by combining an extended Kalman filter and a factor graph optimization. The method is suitable for wheel robot positioning and mapping in a dynamic interference and feature degradation scene.

Description

Technical Field

[0001] This application relates to the field of mobile robot localization and mapping technology, specifically to a laser inertial navigation wheel speed fusion localization and mapping method and system for dynamic degradation environment. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a core technology for autonomous navigation in mobile robots, autonomous vehicles, and intelligent inspection systems. Its fundamental task is to estimate the robot's pose in real time using sensors in unknown environments while simultaneously building a consistent map of the surrounding environment, providing a foundation for subsequent path planning and autonomous decision-making. As robot applications expand to complex indoor and outdoor environments, higher demands are placed on the real-time performance, robustness, and long-term consistency of SLAM systems.

[0003] Existing SLAM technologies mainly include visual SLAM, laser SLAM, and inertial navigation methods based on inertial measurement units (IMUs). Among them, laser SLAM is widely used due to its high-precision perception of environmental geometry. However, a single laser sensor is prone to mismatches in areas with dense dynamic objects, and in sparse or degraded scenarios (such as long corridors, open halls, and tunnels), it can lead to observation degradation or even localization failure. IMUs can provide high-frequency motion measurement, but they suffer from integral drift. Wheel encoders (wheel speed odometers) can provide reliable planar motion constraints for differential-driven wheeled robots, but they are affected by factors such as wheel slippage and uneven road surfaces, resulting in long-term cumulative errors. Traditional multi-sensor fusion schemes often use loosely coupled or filtered frameworks, making it difficult to fully exploit the complementarity of data from different sensors, resulting in insufficient robustness in complex dynamic and degraded environments.

[0004] To meet the needs of wheeled robot applications, there is an urgent need for a tightly coupled multi-sensor SLAM system that can simultaneously cope with dynamic disturbances and feature degradation. Summary of the Invention

[0005] This application provides a laser inertial navigation wheel speed fusion positioning and mapping method and system for dynamic degradation environments, in order to overcome the limitations of existing methods in complex dynamic and degradation environments, and to provide wheeled robots with high-precision and robust real-time positioning and mapping capabilities.

[0006] According to the first aspect, this application provides a laser-inertial navigation wheel velocity fusion positioning and mapping method for dynamic degradation environments, including:

[0007] S1. Data Acquisition and Synchronization: Synchronously acquire raw data from lidar, IMU, and wheel encoder, including point cloud, angular velocity and acceleration measurements, and left and right wheel displacement increments; align the time reference through network time protocol, apply linear interpolation to process time deviations, remove lidar anomalies, correct IMU zero bias, and perform low-pass filtering on wheel speed data;

[0008] S2, IMU and wheel speed pre-integration: The median integration method is used to pre-integrate the inter-frame IMU measurements to calculate the relative rotation, quaternion, velocity and position increments, and to consider gravity compensation; the wheel speed data is pre-integrated based on the differential drive model to obtain the planar displacement and yaw increments, and noise covariance is introduced to provide accurate priors for distortion removal and constraints.

[0009] S3, Dynamic Point Cloud Removal: Transform the current point cloud to the world coordinate system, construct a 3D voxel grid map, update the occupancy probability through Bayesian occupancy probability and compare the occupancy difference with the historical static map, combine the time decay factor to identify and filter out dynamic points and slowly moving targets, and output a static point cloud.

[0010] S4. Point cloud distortion correction: Using the IMU pre-integration results, linear interpolation is performed based on the relative acquisition time of each laser point to calculate the relative attitude at the corresponding time, and the points are projected back to the frame start time to effectively correct motion distortion during scanning and generate a static point cloud with consistent time.

[0011] S5. Environmental Feature Assessment and Adaptive Point Cloud Registration: First, edge and planar features are extracted using local curvature, and a richness score is calculated by combining the feature point ratio, spatial distribution uniformity, and geometric degradation. Then, the weights of the feature matching method and the direct method are adaptively allocated using the sigmoid function, and a registration strategy is decided: efficient feature matching is prioritized in feature-rich scenes, while robust direct methods are adopted in feature-sparse or degraded scenes, and a weighted hybrid method is used in the transition region. Finally, the initial pose is optimized to obtain the registration result.

[0012] S6. Front-end odometer fusion: Using extended Kalman filter tightly coupled with laser registration results, IMU pre-integration and wheel speed pre-integration: First, the state is predicted based on the motion model, and then the residuals of each sensor are fused for updating, outputting fast front-end attitude estimation and uncertainty covariance, providing high-quality initialization for back-end optimization;

[0013] S7. Backend Factor Graph Optimization: Construct a factor graph to integrate laser residuals, IMU pre-integration residuals, wheel speed pre-integration residuals, and loop closure constraints into the optimization framework; solve the maximum a posteriori estimate using the incremental smoothing algorithm to obtain the optimized pose and updated map;

[0014] S8. Map Maintenance and Output: Based on the optimized pose, the distortion-free static point cloud is inserted into the global map through voxel filtering, and the occupied grid is updated to assist subsequent dynamic detection. If the map size is too large, sub-map segmentation and fusion are performed, and the real-time pose trajectory and complete 3D map are finally output.

[0015] Furthermore, S1, data acquisition and synchronization, specifically includes:

[0016] The system acquires 3D laser point clouds from lidar, angular velocity and linear acceleration measurements from IMU, and displacement increments of the left and right wheels from wheel encoders, recording their respective timestamps.

[0017] Synchronize the time base of all sensors and align the data to a unified laser frame timestamp;

[0018] For data with time skew, apply linear interpolation:

[0019]

[0020] in, To synchronize the angular velocity, For adjacent sampling times of the IMU, It is the target synchronization timestamp, between and between, This is the original angular velocity measurement value;

[0021] The data is validated, including removing NaN values ​​or outliers from the laser point cloud, correcting the zero bias of the IMU, and using a low-pass filter to filter noise in the wheel speed.

[0022] Final output , No. Data set after frame laser point cloud synchronization.

[0023] Furthermore, the S2, IMU, and wheel speed pre-integration specifically include:

[0024] The IMU measurements between two laser frames were pre-integrated using the median integration method to calculate relative rotation, quaternions, velocity, and position increments, taking gravity compensation into account.

[0025]

[0026]

[0027]

[0028] In the formula, For relative quaternion rotation, For velocity and position increments, It is the first Angular velocity and acceleration of each IMU It is the accelerometer bias and gyroscope bias. For the cumulative rotation matrix, It is a gravity vector. The number of samples, The sampling interval;

[0029] Pre-integration of wheel speed data based on the differential drive model yields planar displacement and yaw increments:

[0030]

[0031]

[0032] in, It is a rotation increment. It is the displacement increment. It is the wheel axle spacing. It refers to the displacement of the left and right wheels.

[0033] Furthermore, S3, dynamic point cloud removal, specifically includes:

[0034] Transform the current point cloud to the world coordinate system, construct a 3D voxel grid map, and use Bayesian update to calculate the occupancy probability of each grid cell:

[0035]

[0036]

[0037] in, It is a grid The probability of occupancy, As a priori, They are grids Current and historical occupancy probabilities To measure likelihood, For probability changes;

[0038] The probability difference between the current and historical static maps is compared. If the change exceeds a preset threshold, it is marked as dynamic and removed. At the same time, a time decay factor is introduced to handle slowly moving objects.

[0039] Furthermore, S4, point cloud distortion correction, specifically includes:

[0040] The static point cloud obtained after removing dynamic points Increment obtained from IMU pre-integration Processing is performed based on the relative acquisition time of each laser point. Perform linear interpolation to calculate relative attitude;

[0041] Distortion correction is completed from the inverse projection point to the start of the frame:

[0042]

[0043] in, The distortion point after correction. It is the relative frame start time The original point cloud, The cumulative rotation and cumulative translation are represented as follows:

[0044]

[0045]

[0046] in, Inter-frame duration express The percentage of time intervals within the frame intervals From frame start to time The relative rotation increment, This is quaternion multiplication. It is the attitude quaternion at the start of the frame. It is the position of the start of the frame. From frame start to time The relative velocity increment and the relative position increment;

[0047] Furthermore, S5, environmental feature assessment and adaptive point cloud registration, specifically includes:

[0048] S501, Feature Extraction: Distorted static point cloud obtained after dynamic point removal and point cloud distortion correction. Calculate the local curvature for each point in the point cloud and extract the edge point set. and plane point set Curvature calculation is based on the neighborhood geometry of a point:

[0049]

[0050] in, For point curvature, For point The local neighborhood point set, These are the coordinates of points. The point cloud is classified based on its curvature values.

[0051] S502. Environmental Feature Assessment: Based on the extracted feature points, and taking into account the feature point ratio, spatial distribution uniformity, and geometric degradation, calculate the environmental feature richness score for the current frame. :

[0052]

[0053] in, To score for richness, As weight, The total number of feature points. Total points The standard deviation of the spatial distribution of feature points. To maximize the expected variance, These are the singular values ​​of the covariance matrix of the feature point cloud;

[0054] S503, Weight Calculation and Adaptive Decision-Making: In the process of feature extraction and feature matching, both individual feature methods and direct methods have certain drawbacks. Based on the calculated environmental feature richness score, the sigmoid function is used to calculate the weights of the feature methods.

[0055]

[0056] in, For the feature method weights, For steepness parameter, The threshold center;

[0057] Subsequently, based on the feature method weights Adaptive decision-making is made based on the value; when At that time, only feature matching was used for registration; At that time, only the direct method was used for registration; At that time, weighted hybrid residual optimization was adopted, but the direct residual point cloud was further downsampled to control computational overhead;

[0058] S504, Point Cloud Registration Optimization: Based on the decision branch, construct a residual function; when using only the feature method, construct feature matching residuals for edge-to-edge distances and plane-to-plane distances; when using only the direct method, construct direct residuals for the distances from all downsampled points to the nearest plane; in the hybrid mode, construct a hybrid residual function, weighting the feature matching residuals and direct residuals. Combination, for the current frame state transformation Perform nonlinear optimization to solve for the initial pose:

[0059] (1) Using only the feature method:

[0060]

[0061] In the formula, This is the optimized initial attitude transformation matrix. For the previously calculated set of edge points and planar points, It is the first The feature residuals of each feature point are as follows:

[0062] Edge points are point-to-line residuals: , The nearest point to the corresponding edge line on the map. This is the direction vector of the edge line;

[0063] A point in a plane is the point-to-plane residual: , This is the unit normal vector of the corresponding plane in the map. It is the closest point on the plane;

[0064] (2) Using only the direct method:

[0065]

[0066] In the formula, From distortion-free static point clouds The point cloud subset obtained by further downsampling, Indicates the first The direct residuals at each point are as follows:

[0067] Same as above, distance from a point to a plane: ;

[0068] (3) Weighted Mixed Mode:

[0069]

[0070] The characteristic method and the direct method are weighted, and the residuals are processed in the same way as above;

[0071] The optimization employs the Levenberg-Marquardt algorithm for iterative convergence. After optimization, it is applied to point cloud applications. The registered point cloud is obtained. .

[0072] Furthermore, the S6 and front-end odometer fusion specifically include:

[0073] State prediction based on motion model fusion of IMU pre-integration and wheel speed pre-integration:

[0074]

[0075] in, To predict the state, For nonlinear motion model functions, The increments are derived from the IMU and wheel speed pre-integration of S2, representing the posterior state of the previous frame.

[0076] Calculate laser, IMU, and wheel speed residuals and perform state correction using Kalman gain:

[0077]

[0078]

[0079] in, For the residuals of each sensor, For Kalman gain, The updated front-end status; the residuals for each sensor are as follows:

[0080] Laser residual for:

[0081]

[0082] in, This is the unit normal vector of a local plane on the map; For map points; The current point after the transformation; The distance from the plane to the origin; This is the rotation matrix for the current attitude;

[0083] IMU residuals :

[0084]

[0085] The first term is rotational error; the second term is velocity error; and the third term is position error. Let be the vector of the imaginary part of the quaternion. and Indicates the preceding and following frames. Inter-frame time;

[0086] Wheel Odometer Residual :

[0087]

[0088] The first term is the in-plane position error; the second term is the yaw angle error; and the yaw increment is... It is obtained from the pre-integral of wheel speed. The z-axis unit vector. This is the logarithmic mapping from quaternions to Lie algebras.

[0089] Furthermore, the optimization of S7 and the backend factor graph specifically includes:

[0090] Added factors include laser factor, IMU factor, wheel odometer factor, and loop closure factor. Loop closures are detected and constraints are added using point cloud descriptors.

[0091] The residuals are defined in the same form as the front end: laser residuals are based on the plane normal vector distance, IMU residuals are based on the pre-integration error, and wheel odometry residuals are based on the plane position and yaw constraints.

[0092] The maximum a posteriori estimate is obtained using the incremental smoothing algorithm:

[0093]

[0094] in, To optimize the state, For the first Each factor residual, For information matrix;

[0095] The optimization process runs periodically, globally adjusting the trajectory to reduce accumulated drift, ultimately yielding the optimized pose. and update the map .

[0096] Furthermore, the S8, map maintenance and output, specifically includes:

[0097] First, the distortion-free static point cloud of the current frame is... Use optimized pose The map is transformed to the world coordinate system, and voxel filtering is applied to remove redundant points to avoid map inflation. Next, the 3D occupancy raster map is updated by fusing current frame occupancy information with historical static maps using Bayesian probabilistic fusion. Finally, the global map is checked. If the number of points or the volume is too large, perform submap segmentation and edge fusion to prevent memory overflow and maintain map continuity.

[0098] The formula for inserting a map is:

[0099]

[0100] in, For global map, To optimize rotation and translation, For point cloud points.

[0101] According to the second aspect, this application provides a laser-inertial navigation wheel velocity fusion positioning and mapping system for dynamic degradation environments, comprising:

[0102] Data acquisition and synchronization module: Acquires raw data from multiple sensors and ensures time consistency; obtains 3D point cloud, angular velocity, acceleration and wheel speed displacement increment from lidar, IMU and wheel encoder, synchronizes the reference through network time protocol and processes deviations by linear interpolation, then verifies and removes anomalies, corrects zero bias and filters noise, and outputs high-quality synchronized data sets to provide a reliable foundation for subsequent processing;

[0103] IMU and wheel speed pre-integration module: pre-integrates the synchronized data to generate motion increment constraints; calculates the relative rotation, velocity and position increments of the IMU using median integration; derives the wheel speed planar displacement and yaw increments based on the differential speed model; propagates the covariance to model uncertainty; and outputs the pre-integration results and their covariance.

[0104] Dynamic point cloud removal module: Filters out dynamic interference in laser point clouds; constructs a voxel grid and compares the difference with the historical map by Bayesian occupancy probability. Dynamic points are removed if they exceed the threshold. At the same time, time decay is introduced to process slowly moving objects and outputs a clean static point cloud.

[0105] Point cloud distortion correction module: compensates for motion distortion during scanning; linearly interpolates relative pose based on point acquisition time and inversely projects it to the frame start time to generate a time-consistent distorted static point cloud;

[0106] Environmental feature assessment and adaptive point cloud registration module: Adaptively completes point cloud registration based on the characteristics of the environment; comprehensively scores feature points based on feature point ratio, spatial distribution uniformity, and geometric degradation, and adaptively allocates feature matching method weights using the sigmoid function: prioritizes efficient feature matching when features are abundant, switches to robust direct methods when features are sparse and degraded, and uses mixed weighting in the transition region; outputs the initial pose and registered point cloud after Levenberg-Marquardt optimization;

[0107] Front-end odometry fusion module: fuses multi-sensor data to generate front-end attitude; predicts state based on motion model, then calculates laser, IMU and wheel speed residuals and updates them with Kalman gain, outputting front-end attitude and its covariance;

[0108] Backend factor graph optimization module: Reduces drift through global optimization; constructs factor graphs, incorporates laser, IMU, wheel speed and loop closure constraints into the factor graph framework, uses incremental smoothing algorithm to solve maximum a posteriori estimation, periodically adjusts the trajectory and updates the map, and outputs optimized attitude and consistent map;

[0109] Map maintenance and output module: integrates and optimizes point clouds and maintains the map in the long term; inserts the current frame point cloud into the global map after optimized pose transformation and voxel filtering, while updating the occupied grid to enhance dynamic detection; finally outputs real-time trajectory and complete 3D map.

[0110] Compared with the prior art, the beneficial effects of this application are as follows:

[0111] The laser inertial navigation wheel speed fusion positioning and mapping method and system proposed in this application have significant advantages in real-world scenarios where complex dynamic interference and feature degradation coexist: By combining a three-dimensional voxel grid with a dynamic point cloud filtering mechanism based on Bayesian occupancy probability and time decay, interference from moving objects and slowly changing targets is effectively eliminated, avoiding positioning jumps or failures caused by dynamic mismatches in traditional laser SLAM; a multi-dimensional feature richness score is used to adaptively allocate feature matching and direct residual weights via a sigmoid function, maintaining efficient registration in structurally rich environments and automatically enhancing robustness in feature-sparse or degraded scenarios, significantly improving positioning stability under extreme conditions; at the same time, differential wheel odometry pre-integration is deeply integrated as a strong plane constraint into the front-end Kalman filter and back-end factor graph optimization, effectively suppressing drift problems during laser degradation; the overall system adopts a tightly coupled two-stage architecture, with all original measurements jointly optimized in a unified state vector, supporting efficient loop closure detection and large-scale map maintenance, achieving a balance between high accuracy and global consistency. This system is particularly suitable for stable autonomous navigation of wheeled mobile platforms in challenging scenarios such as dense crowds, long corridors, open areas, and tunnels, comprehensively improving robustness, accuracy, and engineering applicability. Attached Figure Description

[0112] Figure 1 This is a schematic diagram of the overall structure of the laser inertial navigation wheel velocity fusion positioning and mapping method under dynamic degradation environment according to an embodiment of this application;

[0113] Figure 2 This is a structural block diagram of the laser-inertial navigation wheel velocity fusion positioning and mapping system under dynamic degradation environment according to an embodiment of this application;

[0114] Figure 3 This is a flowchart illustrating the dynamic point removal process based on a three-dimensional voxel grid, as described in an embodiment of this application.

[0115] Figure 4 This is a flowchart illustrating the environmental feature assessment and adaptive point cloud registration process in an embodiment of this application.

[0116] Figure 5 This is a schematic diagram of the physical structure of an electronic device according to an embodiment of this application. Detailed Implementation

[0117] The present application will now be described in further detail with reference to the accompanying drawings and specific embodiments. Similar elements in different embodiments are referred to by related similar element reference numerals. In the following embodiments, many details are described to facilitate a better understanding of the present application. However, those skilled in the art will readily recognize that some features may be omitted in different situations, or may be replaced by other elements, materials, or methods. In some cases, certain operations related to the present application are not shown or described in the specification. This is to avoid obscuring the core parts of the present application with excessive description. For those skilled in the art, detailed description of these related operations is not necessary; they can fully understand the related operations based on the description in the specification and general technical knowledge in the art.

[0118] Furthermore, the features, operations, or characteristics described in the specification can be combined in any suitable manner to form various embodiments. At the same time, the steps or actions in the method description can be rearranged or adjusted in a manner obvious to those skilled in the art. Therefore, the various orders in the specification and drawings are only for the clear description of a particular embodiment and do not imply a necessary order, unless otherwise stated that a particular order must be followed.

[0119] like Figure 1 As shown, this application provides a laser-inertial navigation wheel speed fusion localization and mapping method and system for dynamic degradation environments, named DA-LIOW-SAM (Dynamic-Adaptive LiDAR-Inertial-Odometry-WheelSLAM). This application significantly improves the localization and mapping capabilities of wheeled robots in challenging scenarios such as dense crowds, long corridors, open areas, and tunnels through the following key innovations: First, as... Figure 3 As shown, a dynamic point cloud filtering module based on a Bayesian occupancy probability update and time decay mechanism using a 3D voxel grid is introduced at the front end. This effectively removes interference from moving objects and slowly changing targets, avoiding localization jumps or failures caused by dynamic mismatches in traditional laser SLAM. Then, as... Figure 4As shown, geometric features are extracted and direct residuals are calculated simultaneously from the distortion-free static point cloud. A multi-dimensional feature richness score (feature ratio, spatial distribution uniformity, geometric degradation) is used, and the weights of the feature matching method and the direct method are adaptively allocated via a sigmoid function. In structurally rich environments, efficient feature registration is prioritized, while in feature-sparse or degraded environments, the robustness of the direct method is automatically enhanced, achieving a dynamic balance between computational efficiency and positioning stability. Secondly, a differential wheel odometer pre-integration factor is deeply integrated into the back-end factor map optimization, providing strong planar motion constraints and significantly suppressing drift problems caused by laser degradation. These innovations are organically combined in a tightly coupled architecture. The front end uses extended Kalman filtering to quickly fuse laser registration, IMU, and wheel speed pre-integration. The back end uniformly optimizes all sensor residuals and loop closure constraints, maintaining global consistency through incremental smoothing. Finally, under optimized posture, voxel filtering is used to insert the data into the map and update the occupied grid.

[0120] Example 1:

[0121] The following combination Figure 1 A specific embodiment of this application will be described in detail below. See also: Figure 1 A laser-inertial navigation wheel velocity fusion positioning and mapping method for dynamic degradation environment, the method comprising the following steps:

[0122] S1. Data Acquisition and Synchronization: Raw data is synchronously acquired from the lidar, IMU, and wheel encoder, including point cloud, angular velocity and acceleration measurements, and left and right wheel displacement increments. The time reference is aligned through a network time protocol, and linear interpolation is applied to handle time deviations. At the same time, lidar anomalies are removed, IMU zero bias is corrected, and wheel speed noise is low-pass filtered to ensure consistent and reliable multi-sensor data.

[0123] Furthermore, S1, data acquisition and synchronization, specifically includes:

[0124] The system acquires 3D laser point clouds from lidar, angular velocity and linear acceleration measurements from IMU, and displacement increments of the left and right wheels from wheel encoders, recording their respective timestamps.

[0125] Synchronize the time base of all sensors and align the data to a unified laser frame timestamp.

[0126] For data with time skew, apply linear interpolation:

[0127]

[0128] in, To synchronize the angular velocity, For adjacent sampling times of the IMU, It is the target synchronization timestamp, between and between, This is the original angular velocity measurement value.

[0129] The data is validated, including removing NaN values ​​or outliers from the laser point cloud, correcting the zero bias of the IMU, and using a low-pass filter to filter noise in the wheel speed.

[0130] Final output , No. Data set after frame laser point cloud synchronization.

[0131] S2, IMU and wheel speed pre-integration: The median integration method is used to pre-integrate the inter-frame IMU measurements to calculate the relative rotation, quaternion, velocity and position increments, and to consider gravity compensation; the wheel speed data is pre-integrated based on the differential drive model to obtain the planar displacement and yaw increments, and noise covariance is introduced to provide accurate priors for distortion removal and constraints.

[0132] Furthermore, the S2, IMU, and wheel speed pre-integration specifically include:

[0133] The IMU measurements between two laser frames were pre-integrated using the median integration method to calculate relative rotation, quaternions, velocity, and position increments, taking gravity compensation into account.

[0134]

[0135]

[0136]

[0137] In the formula, For relative quaternion rotation, For velocity and position increments, It is the first Angular velocity and acceleration of each IMU It is the accelerometer bias and gyroscope bias. For the cumulative rotation matrix, It is a gravity vector. The number of samples, The sampling interval is denoted as .

[0138] Pre-integration of wheel speed data based on the differential drive model yields planar displacement and yaw increments:

[0139]

[0140]

[0141] in, It is a rotation increment. It is the displacement increment. It is the wheel axle spacing. It refers to the displacement of the left and right wheels.

[0142] S3, Dynamic Point Cloud Removal: Transform the current point cloud to the world coordinate system, construct a 3D voxel grid map, update the occupancy probability through Bayesian occupancy probability and compare the occupancy difference with the historical static map, combine the time decay factor to identify and filter out dynamic points and slowly moving targets, and output a high-quality, clean static point cloud.

[0143] Furthermore, S3, dynamic point cloud removal, specifically includes:

[0144] Transform the current point cloud to the world coordinate system, construct a 3D voxel grid map, and use Bayesian update to calculate the occupancy probability of each grid cell:

[0145]

[0146]

[0147] in, It is a grid The probability of occupancy, As a priori, They are grids Current and historical occupancy probabilities To measure likelihood, This represents a change in probability.

[0148] The probability difference between the current and historical static maps is compared. If the change exceeds a preset threshold, it is marked as dynamic and removed. At the same time, a time decay factor is introduced to handle slowly moving objects.

[0149] S4. Point cloud distortion correction: Using the IMU pre-integration results, linear interpolation is performed based on the relative acquisition time of each laser point to calculate the relative attitude at the corresponding time, and the points are projected back to the frame start time to effectively correct motion distortion during scanning and generate a static point cloud with consistent time.

[0150] Furthermore, S4, point cloud distortion correction, specifically includes:

[0151] The static point cloud obtained after removing dynamic points Increment obtained from IMU pre-integration Processing is performed based on the relative acquisition time of each laser point. Linear interpolation is used to calculate the relative attitude.

[0152] Distortion correction is completed from the inverse projection point to the start of the frame:

[0153]

[0154] in, The distortion point after correction. It is the relative frame start time The original point cloud, The cumulative rotation and cumulative translation are represented as follows:

[0155]

[0156]

[0157] in, Inter-frame duration express The percentage of time intervals within the frame intervals From frame start to time The relative rotation increment, This is quaternion multiplication. It is the attitude quaternion at the start of the frame. It is the position of the start of the frame. From frame start to time The relative velocity increment and the relative position increment.

[0158] S5. Environmental Feature Assessment and Adaptive Point Cloud Registration: First, edge and planar features are extracted using local curvature, and a richness score is calculated by combining the feature point ratio, spatial distribution uniformity, and geometric degradation. Then, the weights of the feature matching method and the direct method are adaptively allocated using the sigmoid function, and a registration strategy is decided: efficient feature matching is prioritized in feature-rich scenes, while robust direct methods are adopted in feature-sparse or degraded scenes, and a weighted hybrid method is used in the transition region. Finally, the initial pose is optimized to obtain the registration result.

[0159] Furthermore, S5, environmental feature assessment and adaptive point cloud registration, specifically includes:

[0160] S501, Feature Extraction. The resulting distortion-corrected static point cloud after dynamic point removal and point cloud distortion correction. Calculate the local curvature for each point in the point cloud and extract the edge point set. and plane point set Curvature calculation is based on the neighborhood geometry of a point:

[0161]

[0162] in, For point curvature, For point The local neighborhood point set, Here are the coordinates of the points. The point cloud is classified based on the curvature values.

[0163] S502. Environmental Feature Assessment. Based on the extracted feature points, and considering the feature point ratio, spatial distribution uniformity, and geometric degradation, the environmental feature richness score of the current frame is calculated. :

[0164]

[0165] in, To score for richness, As weight, The total number of feature points. Total points The standard deviation of the spatial distribution of feature points. To maximize the expected variance, These are the singular values ​​of the covariance matrix of the feature point cloud.

[0166] S503, Weight Calculation and Adaptive Decision-Making. In the process of feature extraction and feature matching, both standalone feature methods and direct methods have certain drawbacks. Based on the calculated environmental feature richness score, the weights of the feature methods are calculated using the sigmoid function.

[0167]

[0168] in, For the feature method weights, For steepness parameter, The threshold center is used.

[0169] Subsequently, based on the feature method weights The value is used for adaptive decision-making. When At that time, only feature matching was used for registration; At that time, only the direct method was used for registration; At that time, weighted hybrid residual optimization was adopted, but the direct residual point cloud was further downsampled to control computational overhead.

[0170] S504, Point Cloud Registration Optimization. Based on the decision branch, construct the residual function. When using only the feature method, construct the feature matching residuals for edge-to-edge distances and plane-to-plane distances; when using only the direct method, construct the direct residuals for the distances from all downsampled points to the nearest plane; in the hybrid mode, construct a hybrid residual function, weighting the feature matching residuals and direct residuals. Combination, for the current frame state transformation Perform nonlinear optimization to solve for the initial pose:

[0171] (1) Using only the feature method:

[0172]

[0173] In the formula, This is the optimized initial attitude transformation matrix. For the previously calculated set of edge points and planar points, It is the first The feature residuals of each feature point are as follows:

[0174] Edge points are point-to-line residuals: , The nearest point to the corresponding edge line on the map. This is the direction vector of the edge line.

[0175] A point in a plane is the point-to-plane residual: , This is the unit normal vector of the corresponding plane in the map. It is the closest point on the plane.

[0176] (2) Using only the direct method:

[0177]

[0178] In the formula, From distortion-free static point clouds The point cloud subset obtained by further downsampling, Indicates the first The direct residuals at each point are as follows:

[0179] Same as above, distance from a point to a plane: .

[0180] (3) Weighted Mixed Mode:

[0181]

[0182] The characteristic method and the direct method are weighted, and the residuals are processed in the same way as above.

[0183] The optimization employs the Levenberg-Marquardt algorithm for iterative convergence. After optimization, it is applied to point cloud applications. The registered point cloud is obtained. .

[0184] S6. Front-end odometer fusion: Using extended Kalman filter tightly coupled with laser registration results, IMU pre-integration and wheel speed pre-integration: First, the state is predicted based on the motion model, and then the residuals of each sensor are fused for updating, outputting fast front-end attitude estimation and uncertainty covariance, providing high-quality initialization for back-end optimization;

[0185] Furthermore, the S6 and front-end odometer fusion specifically include:

[0186] State prediction based on motion model fusion of IMU pre-integration and wheel speed pre-integration:

[0187]

[0188] in, To predict the state, For nonlinear motion model functions, The increments are derived from the IMU and wheel speed pre-integration of S2, representing the posterior state of the previous frame.

[0189] Calculate laser, IMU, and wheel speed residuals and perform state correction using Kalman gain:

[0190]

[0191]

[0192] in, For the residuals of each sensor, For Kalman gain, This is the updated front-end state. The residuals for each sensor are as follows:

[0193] Laser residual for:

[0194]

[0195] in, This is the unit normal vector of a local plane on the map; For map points; The current point after the transformation; The distance from the plane to the origin; This is the rotation matrix for the current attitude.

[0196] IMU residuals :

[0197]

[0198] The first term is rotational error; the second term is velocity error; and the third term is position error. Let be the vector of the imaginary part of the quaternion. and Indicates the preceding and following frames. This refers to the inter-frame time.

[0199] Wheel Odometer Residual :

[0200]

[0201] The first term is the in-plane position error; the second term is the yaw angle error; and the yaw increment is... It is obtained from the pre-integral of wheel speed. The z-axis unit vector. This is the logarithmic mapping from quaternions to Lie algebras.

[0202] S7. Backend Factor Graph Optimization: Construct a factor graph to integrate laser residual, IMU pre-integration residual, wheel speed pre-integration residual, and loop closure constraint into the optimization framework; solve the maximum a posteriori estimate through the incremental smoothing algorithm to achieve global trajectory consistency and drift suppression;

[0203] Furthermore, the optimization of S7 and the backend factor graph specifically includes:

[0204] Added factors include laser factor, IMU factor, wheel odometer factor, and loop closure factor. Loop closures are detected and constraints are added using point cloud descriptors.

[0205] The residuals are defined in the same form as the front end: laser residuals are based on the plane normal vector distance, IMU residuals are based on the pre-integration error, and wheel odometry residuals are based on the plane position and yaw constraints.

[0206] The maximum a posteriori estimate is obtained using the incremental smoothing algorithm:

[0207]

[0208] in, To optimize the state, For the first Each factor residual, This is an information matrix.

[0209] The optimization process runs periodically, globally adjusting the trajectory to reduce accumulated drift, ultimately yielding the optimized pose. and update the map .

[0210] S8. Map Maintenance and Output: Based on the optimized pose, the distortion-free static point cloud is inserted into the global map after voxel filtering, while the occupied grid is updated to assist subsequent dynamic detection; if the map size is too large, sub-map segmentation and fusion are performed, and finally, the real-time pose trajectory and complete 3D map are output, ensuring memory efficiency and map size reduction. Figure 1 To the point of being responsive.

[0211] Furthermore, the S8, map maintenance and output, specifically includes:

[0212] First, the distortion-free static point cloud of the current frame is... Use optimized pose The map is transformed to the world coordinate system, and voxel filtering is applied to remove redundant points to avoid map inflation. Next, the 3D occupancy raster map is updated by using Bayesian probabilistic fusion of current frame occupancy information and historical static maps. Finally, the global map is checked. If the number of points or the volume is too large, perform submap segmentation and edge fusion to prevent memory overflow and maintain map continuity.

[0213] The formula for inserting a map is:

[0214]

[0215] in, For global map, To optimize rotation and translation, For point cloud points.

[0216] This embodiment presents a laser-inertial-velocity (LIV) and wheel-speed fusion positioning and mapping method for dynamic degradation environments. It achieves high-precision, robust real-time positioning and 3D map construction by deeply tightly coupling and fusing raw measurements from a 3D LiDAR, inertial measurement unit (IMU), and wheel encoder. The core of this method lies in the following: First, the front end synchronizes multi-sensor data and performs IMU and wheel-speed pre-integration. Then, a 3D voxel grid combined with Bayesian occupancy probability and time decay mechanisms is used to filter out dynamic object interference, obtaining a clean static point cloud. After correcting motion distortion using IMU pre-integration, geometric features are simultaneously extracted from the point cloud, and direct residuals are calculated. Multi-dimensional feature richness scoring is used, and the weights of feature matching and the direct method are adaptively allocated via a sigmoid function. In structurally rich environments, efficient registration is prioritized, while robustness is automatically enhanced in feature-sparse or degradation scenarios. The front end uses extended Kalman filtering to quickly fuse laser, IMU, and wheel-speed pre-integration to output the initial attitude. The back end uses factor graphs to uniformly optimize all sensor residuals and loop closure constraints, maintaining global consistency through incremental smoothing. Finally, the optimized attitude is used to insert the data into the map via voxel filtering and update the occupancy grid. This system effectively overcomes the problems of mismatch, observation degradation and drift in complex scenarios such as dynamic crowds, long corridors, open areas and tunnels in traditional laser SLAM, and significantly improves positioning accuracy, long-term stability and environmental adaptability.

[0217] Example 2:

[0218] Corresponding to the laser inertial navigation wheel velocity fusion positioning and mapping method disclosed in Embodiment 1 under dynamic degradation environment, this application discloses a laser inertial navigation wheel velocity fusion positioning and mapping system under dynamic degradation environment. See [link to embodiment]. Figure 2 The system includes:

[0219] Data Acquisition and Synchronization Module: Acquires raw data from multiple sensors and ensures time consistency. It obtains 3D point clouds, angular velocities, accelerations, and wheel speed displacement increments from LiDAR, IMU, and wheel encoders. It synchronizes the reference data via a network time protocol and processes deviations through linear interpolation. Subsequently, it verifies and removes anomalies, corrects zero bias, and filters noise, outputting high-quality synchronized data sets to provide a reliable foundation for subsequent processing.

[0220] IMU and wheel speed pre-integration module: Pre-integrates the synchronized data to generate motion increment constraints. It uses median integration to calculate the IMU's relative rotation, velocity, and position increments, derives wheel speed planar displacement and yaw increments based on the differential speed model, propagates covariance to model uncertainties, and outputs the pre-integration results and their covariance.

[0221] Dynamic point cloud removal module: Filters out dynamic interference in laser point clouds. It constructs a voxel grid and compares the differences between the Bayesian occupancy probability and the historical map. Dynamic points that exceed the threshold are removed. At the same time, time decay is introduced to process slowly moving objects, and a clean static point cloud is output.

[0222] Point cloud distortion correction module: compensates for motion distortion during scanning. It linearly interpolates the relative pose based on the point acquisition time and inversely projects it to the frame start time to generate a time-consistent distorted static point cloud;

[0223] Environmental Feature Assessment and Adaptive Point Cloud Registration Module: Adaptively completes point cloud registration based on the characteristics of the environment. It comprehensively scores feature points based on proportion, spatial distribution uniformity, and geometric degradation, and adaptively assigns weights to the feature matching method using the sigmoid function: prioritizing efficient feature matching when features are abundant, switching to robust direct methods when features are sparse and degenerate, and using a mixed weighting in the transition region. The initial pose and registered point cloud are output after Levenberg-Marquardt optimization.

[0224] Front-end odometry fusion module: Fuses multi-sensor data to generate front-end attitude. Based on the motion model, the state is predicted, then the residuals from laser, IMU, and wheel speeds are calculated and updated using Kalman gain, outputting the front-end attitude and its covariance.

[0225] Backend factor graph optimization module: Reduces drift through global optimization. Constructs a factor graph, incorporating laser, IMU, wheel speed, and loop closure constraints into the factor graph framework. Uses incremental smoothing algorithm to solve for maximum a posteriori estimation, periodically adjusts the trajectory and updates the map, and outputs an optimized attitude and consistent map;

[0226] Map Maintenance and Output Module: Integrates and optimizes point clouds and maintains the map long-term. It inserts the current frame's point cloud into the global map after optimized pose transformation and voxel filtering, while simultaneously updating occupied grids and enhancing dynamic detection. The final output is a real-time trajectory and a complete 3D map.

[0227] It should be noted that for a detailed description of the laser inertial navigation wheel velocity fusion positioning and mapping system provided in the embodiments of this application, please refer to the relevant description of the laser inertial navigation wheel velocity fusion positioning and mapping method provided in the embodiments of this application, which will not be repeated here.

[0228] In addition, embodiments of this application also provide an electronic device for laser-inertial navigation wheel speed fusion positioning and mapping under dynamic degradation environments. Figure 5 This is a schematic diagram of the physical structure of an electronic device provided in an embodiment of this application. The electronic device may include: a processor 301, a communication interface 302, a memory 303, and a bus 304. The processor 301, communication interface 302, and memory 303 communicate with each other via the bus 304. The processor 301 can call a computer program stored in the memory 303 and executable on the processor 301 to perform the laser inertial navigation wheel velocity fusion positioning and mapping method provided in the above embodiment under dynamic degradation conditions.

[0229] Furthermore, the logical instructions in the aforementioned memory 303 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solutions of this application embodiment, essentially, or the parts that contribute to the prior art, or parts of the technical solutions, can be embodied in the form of software products. These computer software products are stored in a storage medium and include several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0230] The above examples illustrate this application only to aid understanding and are not intended to limit its scope. Those skilled in the art to which this application pertains can make various simple deductions, modifications, or substitutions based on the ideas presented.

Claims

1. A laser-inertial navigation wheel velocity fusion positioning and mapping method for dynamic degradation environments, characterized in that, include: S1. Data Acquisition and Synchronization: Synchronously acquire raw data from lidar, IMU, and wheel encoder, including point cloud, angular velocity and acceleration measurements, and left and right wheel displacement increments; align the time reference through network time protocol, apply linear interpolation to process time deviations, remove lidar anomalies, correct IMU zero bias, and perform low-pass filtering on wheel speed data; S2, IMU and wheel speed pre-integration: The median integration method is used to pre-integrate the inter-frame IMU measurements to calculate the relative rotation, quaternion, velocity and position increments, and to consider gravity compensation; the wheel speed data is pre-integrated based on the differential drive model to obtain the planar displacement and yaw increments, and noise covariance is introduced to provide accurate priors for distortion removal and constraints. S3, Dynamic Point Cloud Removal: Transform the current point cloud to the world coordinate system, construct a 3D voxel grid map, update the occupancy probability through Bayesian occupancy probability and compare the occupancy difference with the historical static map, combine the time decay factor to identify and filter out dynamic points and slowly moving targets, and output a static point cloud. S4. Point cloud distortion correction: Using the IMU pre-integration results, linear interpolation is performed based on the relative acquisition time of each laser point to calculate the relative attitude at the corresponding time, and the points are projected back to the frame start time to effectively correct motion distortion during scanning and generate a static point cloud with consistent time. S5. Environmental Feature Assessment and Adaptive Point Cloud Registration: First, edge and planar features are extracted using local curvature, and a richness score is calculated by combining the feature point ratio, spatial distribution uniformity, and geometric degradation. Then, the weights of the feature matching method and the direct method are adaptively allocated using the sigmoid function, and a registration strategy is decided: efficient feature matching is prioritized in feature-rich scenes, while robust direct methods are adopted in feature-sparse or degraded scenes, and a weighted hybrid method is used in the transition region. Finally, the initial pose is optimized to obtain the registration result. S6. Front-end odometer fusion: Using extended Kalman filter tightly coupled with laser registration results, IMU pre-integration and wheel speed pre-integration: First, the state is predicted based on the motion model, and then the residuals of each sensor are fused for updating, outputting fast front-end attitude estimation and uncertainty covariance, providing high-quality initialization for back-end optimization; S7. Backend Factor Graph Optimization: Construct a factor graph to integrate laser residuals, IMU pre-integration residuals, wheel speed pre-integration residuals, and loop closure constraints into the optimization framework; solve the maximum a posteriori estimate using the incremental smoothing algorithm to obtain the optimized pose and updated map; S8. Map Maintenance and Output: Based on the optimized pose, the distortion-free static point cloud is inserted into the global map through voxel filtering, and the occupied grid is updated to assist subsequent dynamic detection. If the map size is too large, sub-map segmentation and fusion are performed, and the real-time pose trajectory and complete 3D map are finally output.

2. The laser inertial navigation wheel velocity fusion positioning and mapping method under dynamic degradation environment according to claim 1, characterized in that, S1, data acquisition and synchronization, specifically includes: The system acquires 3D laser point clouds from lidar, angular velocity and linear acceleration measurements from IMU, and displacement increments of the left and right wheels from wheel encoders, recording their respective timestamps. Synchronize the time base of all sensors and align the data to a unified laser frame timestamp; For data with time skew, apply linear interpolation: in, To synchronize the angular velocity, For adjacent sampling times of the IMU, It is the target synchronization timestamp, between and between, This is the original angular velocity measurement value; The data is validated, including removing NaN values ​​or outliers from the laser point cloud, correcting the zero bias of the IMU, and using a low-pass filter to filter noise in the wheel speed. Final output , No. Data set after frame laser point cloud synchronization.

3. The laser inertial navigation wheel velocity fusion positioning and mapping method under dynamic degradation environment according to claim 1, characterized in that, The S2, IMU, and wheel speed pre-integral specifically include: Pre-integrate the IMU measurements between two laser frames to calculate relative rotation, quaternions, velocity, and position increments, taking gravity compensation into account: In the formula, For relative quaternion rotation, For velocity and position increments, It is the first Angular velocity and acceleration of each IMU It is the accelerometer bias and gyroscope bias. For the cumulative rotation matrix, It is a gravity vector. For the number of samples, The sampling interval; Pre-integration of wheel speed data based on the differential drive model yields planar displacement and yaw increments: in, It is a rotation increment. It is the displacement increment. It is the wheel axle spacing. It refers to the displacement of the left and right wheels.

4. The laser inertial navigation wheel velocity fusion positioning and mapping method under dynamic degradation environment according to claim 1, wherein S3, dynamic point cloud removal, specifically includes: Transform the current point cloud to the world coordinate system, construct a 3D voxel grid map, and use Bayesian update to calculate the occupancy probability of each grid cell: in, It is a grid The probability of occupancy, As a priori, They are grids Current and historical occupancy probabilities To measure likelihood, For probability changes; The probability difference between the current and historical static maps is compared. If the change exceeds a preset threshold, it is marked as dynamic and removed. At the same time, a time decay factor is introduced to handle slowly moving objects.

5. The laser-inertial navigation wheel velocity fusion positioning and mapping method under dynamic degradation environment according to claim 1, characterized in that, S4, point cloud distortion correction, specifically includes: The static point cloud obtained after removing dynamic points Increment obtained from IMU pre-integration Processing is performed based on the relative acquisition time of each laser point. Perform linear interpolation to calculate relative attitude; Distortion correction is completed from the inverse projection point to the start of the frame: in, The distortion point after correction. It is the relative frame start time The original point cloud, The cumulative rotation and cumulative translation are represented as follows: in, Inter-frame duration express The percentage of time intervals within the frame intervals From frame start to time The relative rotation increment, This is quaternion multiplication. It is the attitude quaternion at the start of the frame. It is the position of the start of the frame. From frame start to time The relative velocity increment and the relative position increment.

6. The laser inertial navigation wheel velocity fusion positioning and mapping method under dynamic degradation environment according to claim 1, characterized in that, The S5, environmental feature assessment and adaptive point cloud registration, specifically includes: S501, Feature Extraction: Distorted static point cloud obtained after dynamic point removal and point cloud distortion correction. Calculate the local curvature for each point in the point cloud and extract the edge point set. and plane point set Curvature calculation is based on the neighborhood geometry of a point: in, For point curvature, For point The local neighborhood point set, Point coordinates are used; the point cloud is classified according to its curvature value. S502. Environmental Feature Assessment: Based on the extracted feature points, and taking into account the feature point ratio, spatial distribution uniformity, and geometric degradation, calculate the environmental feature richness score for the current frame. : in, To score for richness, As weight, The total number of feature points. Total points The standard deviation of the spatial distribution of feature points. To maximize the expected variance, These are the singular values ​​of the covariance matrix of the feature point cloud; S503, Weight Calculation and Adaptive Decision-Making: In the process of feature extraction and feature matching, both individual feature methods and direct methods have certain drawbacks. Based on the calculated environmental feature richness score, the sigmoid function is used to calculate the weights of the feature methods. in, For the feature method weights, For steepness parameter, The threshold center; Subsequently, based on the feature method weights Adaptive decision-making is made based on the value; when At that time, only feature matching was used for registration; At that time, only the direct method was used for registration; At that time, weighted hybrid residual optimization was adopted, but the direct residual point cloud was further downsampled to control computational overhead; S504, Point Cloud Registration Optimization: Based on the decision branch, construct a residual function; when using only the feature method, construct feature matching residuals for edge-to-edge distances and plane-to-plane distances; when using only the direct method, construct direct residuals for the distances from all downsampled points to the nearest plane; in the hybrid mode, construct a hybrid residual function, weighting the feature matching residuals and direct residuals. Combination, for the current frame state transformation Perform nonlinear optimization to solve for the initial pose: (1) Characteristic method: In the formula, This is the optimized initial attitude transformation matrix. For the previously calculated set of edge points and planar points, It is the first The feature residuals of each feature point are as follows: Edge points are point-to-line residuals: , The nearest point to the corresponding edge line on the map. This is the direction vector of the edge line; A point in a plane is the point-to-plane residual: , This is the unit normal vector of the corresponding plane in the map. It is the closest point on the plane; (2) Direct method: In the formula, From distortion-free static point clouds The point cloud subset obtained by further downsampling, Indicates the first The direct residuals at each point are as follows: Same as above, distance from a point to a plane: ; (3) Weighted Mixed Mode: The characteristic method and the direct method are weighted, and the residuals are processed in the same way as above; The optimization employs the Levenberg-Marquardt algorithm for iterative convergence. After optimization, it is applied to point cloud applications. The registered point cloud is obtained. .

7. The laser-inertial navigation wheel velocity fusion positioning and mapping method under dynamic degradation environment according to claim 1, characterized in that, The S6 and front-end odometer fusion specifically include: State prediction based on motion model fusion of IMU pre-integration and wheel speed pre-integration: in, To predict the state, For nonlinear motion model functions, The increments are derived from the IMU and wheel speed pre-integration of S2, representing the posterior state of the previous frame. Calculate laser, IMU, and wheel speed residuals and perform state correction using Kalman gain: in, For the residuals of each sensor, For Kalman gain, The updated front-end status; the residuals for each sensor are as follows: Laser residual for: in, This is the unit normal vector of a local plane on the map; For map points; The current point after the transformation; The distance from the plane to the origin; This is the rotation matrix for the current attitude; IMU residuals : The first term is rotational error; the second term is velocity error; and the third term is position error. Let be the vector of the imaginary part of the quaternion. and Indicates the preceding and following frames. Inter-frame time; Wheel Odometer Residual : The first term is the in-plane position error; the second term is the yaw angle error; and the yaw increment is... It is obtained from the pre-integral of wheel speed. The z-axis unit vector. This is the logarithmic mapping from quaternions to Lie algebras.

8. The laser inertial navigation wheel velocity fusion positioning and mapping method under dynamic degradation environment according to claim 1, characterized in that, The S7 and backend factor graph optimization specifically include: Added factors include laser factor, IMU factor, wheel odometer factor, and loop closure factor. Loop closures are detected and constraints are added using point cloud descriptors. The residuals are defined in the same form as the front end: laser residuals are based on the plane normal vector distance, IMU residuals are based on the pre-integration error, and wheel odometry residuals are based on the plane position and yaw constraints. The maximum a posteriori estimate is obtained using the incremental smoothing algorithm: in, To optimize the state, For the first Each factor residual, For information matrix; The optimization process runs periodically, globally adjusting the trajectory to reduce accumulated drift, ultimately yielding the optimized pose. and update the map .

9. The laser-inertial navigation wheel velocity fusion positioning and mapping method under dynamic degradation environment according to claim 1, characterized in that, The S8, map maintenance and output, specifically includes: First, the distortion-free static point cloud of the current frame is... Use optimized pose The map is transformed to the world coordinate system, and voxel filtering is applied to remove redundant points to avoid map inflation. Next, the 3D occupancy raster map is updated by fusing current frame occupancy information with historical static maps using Bayesian probabilistic fusion. Finally, the global map is checked. If the number of points or the volume is too large, perform submap segmentation and edge fusion to prevent memory overflow and maintain map continuity. The formula for inserting a map is: in, For global map, To optimize rotation and translation, For point cloud points.

10. A laser-inertial navigation wheel velocity fusion positioning and mapping system for dynamic degradation environments, characterized in that, include: Data acquisition and synchronization module: Acquires raw data from multiple sensors and ensures time consistency; 3D point cloud, angular velocity, acceleration and wheel speed displacement increment are acquired from lidar, IMU and wheel encoder. The reference is synchronized through network time protocol and the deviation is processed by linear interpolation. Then, anomalies are removed, zero bias is corrected and noise is filtered to output high-quality synchronized data sets, which provide a reliable basis for subsequent processing. IMU and wheel speed pre-integration module: pre-integrates the synchronized data to generate motion increment constraints; The relative rotation, velocity, and position increments of the IMU are calculated using median integral. The wheel speed planar displacement and yaw increments are obtained based on the differential speed model. The uncertainty is modeled by propagating the covariance, and the pre-integration results and their covariance are output. Dynamic point cloud removal module: Filters out dynamic interference in laser point clouds; constructs a voxel grid and compares the difference with the historical map by Bayesian occupancy probability. If the difference exceeds the threshold, dynamic points are removed. At the same time, time decay is introduced to process slowly moving objects and outputs a clean static point cloud. Point cloud distortion correction module: compensates for motion distortion during scanning; linearly interpolates relative pose based on point acquisition time and inversely projects it to the frame start time to generate a time-consistent distorted static point cloud; Environmental feature assessment and adaptive point cloud registration module: Adaptively completes point cloud registration based on the characteristics of the environment; comprehensively scores feature points based on feature point ratio, spatial distribution uniformity, and geometric degradation, and adaptively allocates feature matching method weights using the sigmoid function: prioritizes efficient feature matching when features are abundant, switches to robust direct methods when features are sparse and degraded, and uses mixed weighting in the transition region; outputs the initial pose and registered point cloud after Levenberg-Marquardt optimization; Front-end odometry fusion module: fuses multi-sensor data to generate front-end attitude; predicts state based on motion model, then calculates laser, IMU and wheel speed residuals and updates them with Kalman gain, outputting front-end attitude and its covariance; Backend factor graph optimization module: Reduces drift through global optimization; constructs factor graphs, incorporates laser, IMU, wheel speed and loop closure constraints into the factor graph framework, uses incremental smoothing algorithm to solve maximum a posteriori estimation, periodically adjusts the trajectory and updates the map, and outputs optimized attitude and consistent map; Map maintenance and output module: integrates and optimizes point clouds and maintains the map in the long term; inserts the current frame point cloud into the global map after optimized pose transformation and voxel filtering, while updating the occupied grid to enhance dynamic detection; finally outputs real-time trajectory and complete 3D map.

Images