Laser slam method, system, device and medium with dynamic global pose constraint

Abstract

The specification discloses a laser SLAM method, system, device and medium with dynamic global attitude constraint, which obtains point cloud data collected by a laser radar, angular velocity and acceleration data collected by an inertial measurement unit (IMU), and position data collected by a global navigation satellite system (GNSS), constructs a LiDAR matching factor by using the point cloud data, constructs an IMU pre-integration factor by using the IMU data for pre-integration, calculates an observation value of a pitch angle and an observation value of a roll angle of a carrier based on the acceleration data, constructs a dynamic attitude constraint factor, establishes a tight coupling graph optimization framework based on a factor graph, models the LiDAR matching factor, the IMU pre-integration factor and the dynamic attitude constraint factor as constraints of the factor graph, adjusts a weight of the dynamic attitude constraint factor in the factor graph according to a state of the GNSS signal and according to a preset rule, solves in real time by using an iSAM2 algorithm, calibrates IMU parameters online, outputs a real-time pose of the carrier, projects the LiDAR point cloud to a world coordinate system, and constructs a global three-dimensional map.

Description

Technical Field

[0001] This invention relates to the field of simultaneous localization and mapping (SLAM) technology, and more specifically, to a laser SLAM method, system, device, and medium with dynamic global attitude constraints. Background Technology

[0002] Laser-based SLAM technology is one of the core technologies for autonomous navigation and environmental perception in autonomous vehicles, mobile robots, drones, and handheld mapping devices. It acquires point cloud data by scanning the surrounding environment with lidar, and combines this with high-frequency attitude and acceleration information provided by an IMU (Integrated Measurement Unit) to construct a 3D map in real time in environments without prior maps, while simultaneously estimating the vehicle's pose.

[0003] However, existing laser SLAM technology suffers from the following key technical shortcomings in practical applications. First, most laser SLAM systems rely excessively on GNSS global constraints. In environments where GNSS signals are obstructed or absent, such as urban canyons, tunnels, underground parking garages, and forests, relative pose estimation relies solely on laser odometry and IMUs. Errors accumulate rapidly over time, leading to severe trajectory drift and even malfunction. Furthermore, lidar primarily estimates pose by matching planar features in the environment, resulting in lower accuracy for pitch and roll angle estimation. This is particularly true in scenarios involving long-distance straight-line travel or environments with uniform features, where pitch and roll angle errors accumulate rapidly, significantly increasing elevation errors. Second, in high-precision and long-term operational scenarios, the effects of Earth's rotation and Coriolis acceleration cannot be ignored. Traditional laser SLAM systems typically neglect these physical effects, leading to systematic errors in attitude calculation, especially pronounced in high-latitude regions and on high-speed moving vehicles.

[0004] Therefore, there is an urgent need to study a laser SLAM method with dynamic global attitude constraints to meet the requirements of long-term, high-precision positioning and mapping in complex environments. Summary of the Invention

[0005] This specification provides a laser SLAM method, system, device, and medium with dynamic global attitude constraints to overcome at least one technical problem existing in related technologies.

[0006] According to a first aspect of the embodiments of this specification, a laser SLAM method with dynamic global attitude constraints is provided, comprising: Acquire point cloud data from lidar, angular velocity and acceleration data from inertial measurement unit (IMU), and position data from global navigation satellite system (GNSS). Perform time synchronization and noise reduction on all sensor data, and perform motion distortion correction on point cloud data. LiDAR matching factors are constructed using corrected point cloud data; IMU pre-integration factors are constructed by pre-integrating IMU data between adjacent key data frames; and dynamic attitude constraint factors are constructed by calculating pitch and roll angle observations of the carrier in static or uniform motion states based on acceleration data. A tightly coupled graph optimization framework based on factor graphs is constructed. The LiDAR matching factor, IMU pre-integration factor, and dynamic attitude constraint factor are modeled as constraints of the factor graph. According to the state of the GNSS signal, the weights of the dynamic attitude constraint factors in the factor graph are adjusted according to preset rules. The iSAM2 algorithm is used for real-time optimization and solution. The IMU parameters are calibrated online, the real-time pose of the carrier is output, and the LiDAR point cloud is projected onto the world coordinate system to construct a global 3D map.

[0007] Preferably, the step of constructing the LiDAR matching factor using the corrected point cloud data includes: Edge and planar features are extracted from LiDAR point cloud data. Feature points in the current frame are matched with feature points in the local map to obtain the LiDAR matching factor. The Voxelized Generalized Iterative Closest Point (VGICP) algorithm is used for point cloud matching, and the residual is defined as: in, For state vectors, and The first Frame and the A frame is a pose in the world coordinate system. The first result obtained by the VGICP algorithm Frame to the Relative pose transformation of frames, Let be the logarithmic mapping from Lie groups to Lie algebras.

[0008] Preferably, the step of pre-integrating the IMU data between adjacent key data frames to construct the IMU pre-integration factor includes: The IMU pre-integration factor is obtained by pre-integrating the IMU data between two keyframes. The residual of the IMU pre-integration is defined as: in, , and These represent the relative rotation, relative velocity, and relative position obtained through IMU pre-integration, respectively. The time interval between two key data frames. Let m be the position (m) of the carrier in the world coordinate system. Let the velocity of the carrier be in the world coordinate system (m / s). Let be the rotation quaternion from the carrier coordinate system to the world coordinate system. This represents the gyroscope's zero-drift offset (rad / s). The accelerometer zero drift offset (m / s²) It is the acceleration due to gravity. Characterizes the transpose of a rotation matrix; By introducing Earth rotation compensation and Coriolis acceleration compensation, systematic integral errors in high-latitude regions or high-speed motion scenarios are corrected, including: Subtract the projected component of the Earth's rotational angular velocity in the IMU coordinate system from the angular velocity measurement. The vector of the Earth's rotational angular velocity in the local horizontal coordinate system is: in, The latitude of the location of the carrier. Let be the Earth's rotational angular velocity, which is a constant value in the inertial coordinate system, with a magnitude of . The direction is along the Earth's rotation axis, pointing towards the North Pole; Subtract the projection of the Earth's rotational angular velocity in the carrier coordinate system from the gyroscope's measurement: in, Let be the projection of the angular velocity of the vehicle coordinate system relative to the local horizontal coordinate system onto the vehicle coordinate system. This is the measurement value from the gyroscope. Let be the rotation matrix from the carrier coordinate system to the local horizontal coordinate system. This is the vector of the Earth's rotational angular velocity in the local horizontal coordinate system; Subtract the centripetal acceleration component and the Coriolis acceleration component caused by the Earth's rotation from the acceleration measurement. The centripetal acceleration caused by the Earth's rotation is: in, Let be the position vector of the carrier in the local horizontal coordinate system. This is the vector of the Earth's rotational angular velocity in the local horizontal coordinate system; The magnitude of the Coriolis acceleration is: in, Let the velocity of the carrier be in the local horizontal coordinate system. This is the vector of the Earth's rotational angular velocity in the local horizontal coordinate system; Subtract the projections of the Coriolis acceleration and centripetal acceleration in the carrier coordinate system from the accelerometer measurements: in, This represents the projection of the acceleration of the vehicle coordinate system relative to the local horizontal coordinate system onto the vehicle coordinate system. This is the measurement value from the accelerometer.

[0009] Preferably, the step of calculating the observed pitch and roll angles of the carrier in a static or uniform motion state based on acceleration data and constructing dynamic attitude constraint factors includes: When the carrier is at rest or in uniform linear motion, the specific force measured by the accelerometer Caused by gravitational acceleration, in, This is the gravitational acceleration vector in the world coordinate system. Characterizes the rotation matrix; Formulas for estimating pitch and roll angles: in, The pitch angle, For roll angle, , and These are the accelerometer readings on the three axes, respectively. To determine the effectiveness of the accelerometer attitude estimation, the magnitude of the vehicle's acceleration is calculated in real time. when When the accelerometer attitude estimation is deemed valid, then... The acceleration threshold is set to 0.1g; When the accelerometer attitude estimation is effective, a dynamic attitude constraint factor is constructed. The residual of the dynamic attitude constraint factor is defined as follows: in, and These are the pitch angle and roll angle in the state vector, respectively; The Jacobian matrix of the dynamic attitude constraint factor is: in, Let be the Jacobian matrix of the residuals in quaternions; A first-order low-pass filter is used to smooth the pitch and roll angles estimated by the accelerometer over time. in, These are the filter coefficients, with values ​​ranging from 0.1 to 0.3.

[0010] Preferably, the steps of constructing a tightly coupled graph optimization framework based on factor graphs, modeling the LiDAR matching factor, IMU pre-integration factor, and dynamic attitude constraint factor as constraints of the factor graph, adjusting the weights of the dynamic attitude constraint factors in the factor graph according to preset rules based on the state of the GNSS signal, performing real-time optimization using the iSAM2 algorithm, calibrating the IMU parameters online, and outputting the real-time pose of the carrier include: The LiDAR matching factor, IMU pre-integration factor, and dynamic attitude constraint factor are modeled as constraints of a factor graph. A tightly coupled graph optimization framework based on the factor graph is constructed. The iSAM2 algorithm is used to incrementally optimize the factor graph, and the real-time pose of the carrier is output. The objective function of the tightly coupled graph optimization framework is: in, For state vectors, The residuals of the LiDAR matching factor. The residuals of the IMU pre-integration factor, The residuals of the GNSS position factors are calculated from GNSS data. The residual of the dynamic attitude constraint factor. This is the corresponding covariance matrix; During the solution process, the online calibration state vector includes gyroscope zero drift offset and accelerometer zero drift offset. Based on the state of the GNSS signal, the weights of the dynamic attitude constraint factors in the factor graph are adjusted according to preset rules.

[0011] Preferably, the step of adjusting the weights of the dynamic attitude constraint factors in the factor graph according to the state of the GNSS signal and a preset rule includes: Obtain the standard deviation of GNSS position measurements Set the baseline covariance and standard deviation threshold ; When GNSS signals are available, calculate the covariance of the current dynamic attitude constraint factor according to the formula. in, The minimum covariance of the attitude constraints corresponds to the maximum weight, set as follows: , The standard deviation of GNSS position measurements. The maximum standard deviation threshold available for GNSS is set to 0.1m. To adjust the coefficient, set it to 10; When GNSS signals are missing, the covariance of the dynamic attitude constraint factor will be... Set to minimum value ; The calculated covariance Time-domain smoothing filtering is performed using a first-order low-pass filter: in, The filter coefficient is set to 0.2.

[0012] Preferably, the online calibration of the state vector, including the steps of gyroscope zero-drift offset and accelerometer zero-drift offset, includes: During factor graph optimization, the gyroscope zero drift offset and accelerometer zero drift offset of the IMU are added to the optimization framework as state variables to be estimated. The estimated value of the IMU zero drift offset is updated according to the constraints provided by the LiDAR matching factor, GNSS position factor and dynamic attitude constraint factor. When the GNSS signal is available, the absolute position and velocity constraints of the GNSS are used to assist the convergence of the IMU zero drift offset. When the GNSS signal is missing, the IMU zero drift offset is observed using the LiDAR matching factor and dynamic attitude constraint factor.

[0013] According to a second aspect of the embodiments of this specification, a laser SLAM system with dynamic global attitude constraints is provided, including a data acquisition module, a factor extraction module, an optimization solution module, and a data output module, wherein... The data acquisition module is configured to acquire point cloud data collected by lidar, angular velocity and acceleration data collected by inertial measurement unit (IMU), and position data collected by global navigation satellite system (GNSS), perform time synchronization and noise reduction on all sensor data, and perform motion distortion correction on point cloud data. The factor extraction module is configured to construct LiDAR matching factors using corrected point cloud data; pre-integrate IMU data between adjacent key data frames to construct IMU pre-integration factors; and calculate pitch and roll angle observations of the carrier in static or uniform motion states based on acceleration data to construct dynamic attitude constraint factors. The optimization solution module is configured to construct a tightly coupled graph optimization framework based on factor graphs, model the LiDAR matching factor, IMU pre-integration factor and dynamic attitude constraint factor as constraints of the factor graph, adjust the weight of the dynamic attitude constraint factor in the factor graph according to the state of the GNSS signal and according to preset rules, perform real-time optimization solution using the iSAM2 algorithm, and calibrate the IMU parameters online. The data output module is configured to output the real-time pose of the carrier, project the LiDAR point cloud onto the world coordinate system, and construct a global 3D map.

[0014] According to a third aspect of the embodiments of this specification, a computing device is provided, including a storage device and a processor, the storage device being used to store a computer program, and the processor running the computer program to cause the computing device to perform the steps of the laser SLAM method with dynamic global attitude constraints.

[0015] According to a fourth aspect of the embodiments of this specification, a storage medium is provided that stores a computer program used in the computing device, which, when executed by a processor, implements the steps of the laser SLAM method with dynamic global attitude constraints.

[0016] The beneficial effects of the embodiments in this specification are as follows: This specification provides a laser SLAM method, system, device, and medium with dynamic global attitude constraints. The method constructs LiDAR matching factors, IMU pre-integration factors, and dynamic attitude constraint factors using point cloud data, IMU data, and GNSS position data. Using multiple factors as constraints, a tightly coupled graph optimization framework based on factor graphs is established. The iSAM2 algorithm is used for real-time optimization and solution. Drift errors are controlled by adaptively adjusting the weights of dynamic attitude constraint factors in the factor graph. IMU parameters are calibrated online, and the real-time pose of the carrier is calculated, thereby constructing a global 3D map. Compared to existing technologies, the use of multiple factor constraints overcomes the deficiency of over-reliance on GNSS global constraints. The dynamic attitude constraint factors improve the estimation accuracy of pitch and roll angles, as well as elevation direction accuracy. Furthermore, compensation for Earth's rotation and Coriolis acceleration is incorporated to effectively control systemic errors, thus meeting the needs of long-term, high-precision positioning and mapping in complex environments.

[0017] The innovative aspects of the embodiments in this specification include: 1. In this specification, a dynamic global attitude constraint factor is constructed, which uses the pitch and roll angle information provided by the IMU accelerometer to form additional global constraints, significantly reducing the dependence on GNSS global constraints and greatly improving the estimation accuracy of pitch and roll angles. The improvement in pitch and roll angle accuracy directly improves the estimation accuracy of elevation direction, which is one of the innovations of the embodiments in this specification.

[0018] 2. In this specification, the LiDAR matching factor, IMU pre-integration factor, and dynamic attitude constraint factor are modeled as constraints of a factor graph. The iSAM2 algorithm is used for real-time optimization and solution. According to the state of the GNSS signal, the weights of the dynamic attitude constraint factors in the factor graph are adjusted according to preset rules. This can efficiently handle incremental updates of the factor graph. The weights of the dynamic attitude constraint factors are adaptively adjusted according to the quality of the GNSS signal, which can effectively suppress trajectory drift. This is one of the innovative points of the embodiments in this specification.

[0019] 3. In this specification, the introduction of Earth rotation compensation and Coriolis acceleration compensation during IMU pre-integration and attitude calculation improves the accuracy of long-term operation and enhances the global consistency of the constructed map. This is one of the innovative points of the embodiments in this specification. Attached Figure Description

[0020] To more clearly illustrate the technical solutions in the embodiments or related technologies of this specification, the drawings used in the description of the embodiments or related technologies will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0021] Figure 1 This is a flowchart illustrating a laser SLAM method with dynamic global attitude constraints, provided as an embodiment of this specification. Figure 2 A schematic diagram of a laser SLAM system with dynamic global attitude constraints provided in one embodiment of this specification; Figure 3 This is a schematic diagram of the structure of a computing device provided in one embodiment of this specification; Figure 4 This is a schematic diagram of the structure of a storage medium provided in one embodiment of this specification. Detailed Implementation

[0022] The technical solutions in the embodiments of this specification 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.

[0023] It should be noted that the terms "comprising" and "having," and any variations thereof, in the embodiments and drawings of this specification are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units is not limited to the steps or units listed, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to these processes, methods, products, or devices.

[0024] This specification discloses a laser SLAM method, system, device, and medium with dynamic global attitude constraints, which will be described in detail below.

[0025] Figure 1 This is a flowchart illustrating a laser SLAM method with dynamic global attitude constraints, provided as an embodiment of this specification. Figure 1 As shown, a laser SLAM method with dynamic global attitude constraints includes: S110 acquires point cloud data collected by lidar, angular velocity and acceleration data collected by inertial measurement unit (IMU), and position data collected by global navigation satellite system (GNSS). It performs time synchronization and noise reduction on all sensor data and performs motion distortion correction on point cloud data.

[0026] LiDAR point clouds typically contain a large number of noisy points, such as outliers, reflection points, and moving object points. The following methods are used for point cloud denoising: statistical filtering, which calculates the average distance between each point and its nearest neighbors and removes outliers with an average distance greater than a threshold; radius filtering, which calculates the number of neighboring points within a specified radius for each point and removes isolated points with fewer than a threshold; and moving object removal, which uses motion information provided by the IMU to detect and remove moving object points in the point cloud.

[0027] Because LiDAR has a limited scanning frequency, typically 10-20Hz, the carrier may move during the scanning of a single point cloud frame, causing motion distortion. Motion distortion correction using IMU data is performed using the following steps: First, record the start time of the LiDAR frame. and end time For the first Each scan point is used to calculate its precise scan time. Next, based on the IMU data, interpolation is performed to obtain... Pose transformation of the time carrier relative to the start time of the LiDAR frame The first Each scan point from LiDAR coordinate system transformation at time t = ... LiDAR coordinate system at time: ,in, For the first Each scan point is in Coordinates in the LiDAR coordinate system at that time. For the corrected point at Coordinates in the LiDAR coordinate system at that time.

[0028] A sliding window averaging filter was used to denoise the IMU data, with a window size of 5 sampling points.

[0029] S120. Construct LiDAR matching factors using the corrected point cloud data; pre-integrate IMU data between adjacent key data frames to construct IMU pre-integration factors; calculate pitch and roll angle observations of the carrier in static or uniform motion states based on acceleration data to construct dynamic attitude constraint factors.

[0030] In a specific embodiment, the step of constructing the LiDAR matching factor using the corrected point cloud data includes: Edge and planar features are extracted from LiDAR point cloud data. Feature points in the current frame are matched with feature points in the local map to obtain the LiDAR matching factor. The Voxelized Generalized Iterative Closest Point (VGICP) algorithm is used for point cloud matching, and the residual is defined as: in, For state vectors, and The first Frame and the A frame is a pose in the world coordinate system. The first result obtained by the VGICP algorithm Frame to the Relative pose transformation of frames, Let be the logarithmic mapping from Lie groups to Lie algebras.

[0031] In a specific embodiment, the step of pre-integrating the IMU data between adjacent key data frames to construct the IMU pre-integration factor includes: The IMU pre-integration factor is obtained by pre-integrating the IMU data between two keyframes. The residual of the IMU pre-integration is defined as: in, , and These represent the relative rotation, relative velocity, and relative position obtained through IMU pre-integration, respectively. The time interval between two key data frames. Let m be the position (m) of the carrier in the world coordinate system. Let the velocity of the carrier be in the world coordinate system (m / s). Let be the rotation quaternion from the carrier coordinate system to the world coordinate system. This represents the gyroscope's zero-drift offset (rad / s). The accelerometer zero drift offset (m / s²) It is the acceleration due to gravity. Characterizes the transpose of a rotation matrix.

[0032] By introducing Earth rotation compensation and Coriolis acceleration compensation, systematic integral errors in high-latitude regions or high-speed motion scenarios are corrected, including: Subtract the projected component of the Earth's rotational angular velocity in the IMU coordinate system from the angular velocity measurement. The vector of the Earth's rotational angular velocity in the local horizontal coordinate system is: in, The latitude of the location of the carrier. Let be the Earth's rotational angular velocity, which is a constant value in the inertial coordinate system, with a magnitude of . The direction is along the Earth's rotation axis, pointing towards the North Pole; Subtract the projection of the Earth's rotational angular velocity in the carrier coordinate system from the gyroscope's measurement: in, Let be the projection of the angular velocity of the vehicle coordinate system relative to the local horizontal coordinate system onto the vehicle coordinate system. This is the measurement value from the gyroscope. Let be the rotation matrix from the carrier coordinate system to the local horizontal coordinate system. This is the vector of the Earth's rotational angular velocity in the local horizontal coordinate system; Subtract the centripetal acceleration component and the Coriolis acceleration component caused by the Earth's rotation from the acceleration measurement. The centripetal acceleration caused by the Earth's rotation is: in, Let be the position vector of the carrier in the local horizontal coordinate system. This is the vector of the Earth's rotational angular velocity in the local horizontal coordinate system; The magnitude of the Coriolis acceleration is: in, Let the velocity of the carrier be in the local horizontal coordinate system. This is the vector of the Earth's rotational angular velocity in the local horizontal coordinate system; Subtract the projections of the Coriolis acceleration and centripetal acceleration in the carrier coordinate system from the accelerometer measurements: in, This represents the projection of the acceleration of the vehicle coordinate system relative to the local horizontal coordinate system onto the vehicle coordinate system. This is the measurement value from the accelerometer.

[0033] During IMU pre-integration and attitude calculation, Earth rotation compensation and Coriolis acceleration compensation are introduced to eliminate systematic errors caused by Earth rotation and improve the accuracy of long-term operation.

[0034] A dynamic attitude constraint factor is added, with pitch and roll angle information provided in real time by the IMU accelerometer, forming additional global attitude constraints. When the vehicle acceleration is small, the specific force measured by the accelerometer is mainly caused by gravitational acceleration, which can be used to estimate the pitch and roll angles.

[0035] In a specific embodiment, the step of calculating the observed pitch and roll angles of the carrier in a static or uniform motion state based on acceleration data and constructing dynamic attitude constraint factors includes: When the carrier is at rest or in uniform linear motion, the specific force measured by the accelerometer Caused by gravitational acceleration, in, This is the gravitational acceleration vector in the world coordinate system. Characterizes the rotation matrix; Formulas for estimating pitch and roll angles: in, The pitch angle, For roll angle, , and These are the accelerometer readings on the three axes, respectively. To determine the effectiveness of the accelerometer attitude estimation, the magnitude of the vehicle's acceleration is calculated in real time. when When the accelerometer attitude estimation is deemed valid, then... The acceleration threshold is set to 0.1g; When the accelerometer attitude estimation is effective, a dynamic attitude constraint factor is constructed. The residual of the dynamic attitude constraint factor is defined as follows: in, and These are the pitch angle and roll angle in the state vector, respectively; The Jacobian matrix of the dynamic attitude constraint factor is: in, Let be the Jacobian matrix of the residuals in quaternions; A first-order low-pass filter is used to smooth the pitch and roll angles estimated by the accelerometer over time. in, These are the filter coefficients, with values ​​ranging from 0.1 to 0.3.

[0036] The factor graph contains four types of factors: LiDAR matching factor, which describes the relative pose relationship between two keyframes, obtained by matching feature points in the current frame with feature points in the local map; IMU pre-integration factor, which describes the relative motion relationship between two adjacent keyframes, obtained by pre-integrating the IMU data between the two keyframes; dynamic attitude constraint factor, which provides global constraints on pitch and roll angles; and GNSS position factor, which provides absolute position constraints, with its residual defined as: ,in, The location is obtained from GNSS measurements.

[0037] S130. Construct a tightly coupled graph optimization framework based on factor graphs. Model the LiDAR matching factor, IMU pre-integration factor, and dynamic attitude constraint factor as constraints of the factor graph. Adjust the weight of the dynamic attitude constraint factor in the factor graph according to the state of the GNSS signal and according to preset rules. Use the iSAM2 algorithm for real-time optimization and solution. Calibrate the IMU parameters online, output the real-time pose of the carrier, project the LiDAR point cloud onto the world coordinate system, and construct a global three-dimensional map.

[0038] The iSAM2 algorithm is used for incremental optimization of the factor graph. Based on a Bayesian tree data structure, the iSAM2 algorithm can efficiently handle incremental updates of the factor graph, avoiding the need to resolve the entire factor graph for each optimization, thus greatly improving the real-time performance of the system. When a new factor is added to the factor graph, the iSAM2 algorithm updates only a portion of the Bayesian tree, and then resolves the state estimate of that portion, thereby achieving real-time incremental optimization.

[0039] In a specific embodiment, the steps of constructing a tightly coupled graph optimization framework based on factor graphs, modeling the LiDAR matching factor, IMU pre-integration factor, and dynamic attitude constraint factor as constraints of the factor graph, adjusting the weights of the dynamic attitude constraint factors in the factor graph according to preset rules based on the state of the GNSS signal, performing real-time optimization using the iSAM2 algorithm, calibrating the IMU parameters online, and outputting the real-time pose of the carrier include: The LiDAR matching factor, IMU pre-integration factor, and dynamic attitude constraint factor are modeled as constraints of a factor graph. A tightly coupled graph optimization framework based on the factor graph is constructed. The iSAM2 algorithm is used to incrementally optimize the factor graph, and the real-time pose of the carrier is output. The objective function of the tightly coupled graph optimization framework is: in, For state vectors, The residuals of the LiDAR matching factor. The residuals of the IMU pre-integration factor, The residuals of the GNSS position factors are calculated from GNSS data. The residual of the dynamic attitude constraint factor. This is the corresponding covariance matrix; During the solution process, the online calibration state vector includes gyroscope zero drift offset and accelerometer zero drift offset. Based on the state of the GNSS signal, the weights of the dynamic attitude constraint factors in the factor graph are adjusted according to preset rules.

[0040] In a specific embodiment, the step of adjusting the weights of the dynamic attitude constraint factors in the factor graph according to the state of the GNSS signal and a preset rule includes: Obtain the standard deviation of GNSS position measurements Set the baseline covariance and standard deviation threshold ; When GNSS signals are available, calculate the covariance of the current dynamic attitude constraint factor according to the formula. in, The minimum covariance of the attitude constraints corresponds to the maximum weight, set as follows: , The standard deviation of GNSS position measurements. The maximum standard deviation threshold available for GNSS is set to 0.1m. To adjust the coefficient, set it to 10; When GNSS signals are missing, the covariance of the dynamic attitude constraint factor will be... Set to minimum value ; The calculated covariance Time-domain smoothing filtering is performed using a first-order low-pass filter: in, The filter coefficient is set to 0.2.

[0041] A dynamic weight adjustment mechanism based on GNSS signal quality is designed, where the weights of attitude constraint factors are automatically adjusted according to GNSS availability. When GNSS signals are available, the weights of attitude constraints are reduced; when GNSS signals are unavailable, the weights of attitude constraints are automatically increased to suppress trajectory drift.

[0042] In a specific embodiment, the step of online calibration of the state vector, including gyroscope zero-drift offset and accelerometer zero-drift offset, includes: During factor graph optimization, the gyroscope zero drift offset and accelerometer zero drift offset of the IMU are added to the optimization framework as state variables to be estimated. The estimated value of the IMU zero drift offset is updated according to the constraints provided by the LiDAR matching factor, GNSS position factor and dynamic attitude constraint factor. When the GNSS signal is available, the absolute position and velocity constraints of the GNSS are used to assist the convergence of the IMU zero drift offset. When the GNSS signal is missing, the IMU zero drift offset is observed using the LiDAR matching factor and dynamic attitude constraint factor.

[0043] The system estimates parameters such as the gyroscope zero drift offset and accelerometer zero drift offset of the IMU online. When GNSS is available, it uses the absolute position information provided by GNSS to enhance the attitude calculation and further improve the accuracy and robustness of the system.

[0044] The system automatically adjusts its parameters based on the motion characteristics of the carrier, enabling universal support for various platforms such as handheld devices, drones, robots, and vehicles. It uses a sliding time window to statistically analyze the carrier's average velocity, maximum angular velocity, and acceleration variance; based on the statistical results, it automatically classifies the carrier into one of the following: handheld device, ground robot, vehicle platform, or drone platform; and adaptively adjusts the keyframe selection frequency, point cloud matching iteration threshold, and low-excitation acceleration threshold for dynamic attitude constraints based on the classification results.

[0045] Figure 2 This is a schematic diagram of a laser SLAM system with dynamic global attitude constraints, provided as an embodiment of this specification. Figure 2 As shown, a laser SLAM system 200 with dynamic global attitude constraints includes a data acquisition module 210, a factor extraction module 220, an optimization solution module 230, and a data output module 240, wherein... The data acquisition module 210 is configured to acquire point cloud data collected by lidar, angular velocity and acceleration data collected by inertial measurement unit (IMU), and position data collected by global navigation satellite system (GNSS), perform time synchronization and noise reduction on all sensor data, and perform motion distortion correction processing on point cloud data.

[0046] The factor extraction module 220 is configured to construct LiDAR matching factors using corrected point cloud data; pre-integrate IMU data between adjacent key data frames to construct IMU pre-integration factors; and calculate pitch and roll angle observations of the carrier in a static or uniform motion state based on acceleration data to construct dynamic attitude constraint factors.

[0047] The optimization solution module 230 is configured to construct a tightly coupled graph optimization framework based on factor graphs, model the LiDAR matching factor, IMU pre-integration factor and dynamic attitude constraint factor as constraints of the factor graph, adjust the weight of the dynamic attitude constraint factor in the factor graph according to the state of the GNSS signal and according to preset rules, perform real-time optimization solution using the iSAM2 algorithm, and calibrate the IMU parameters online.

[0048] The data output module 240 is configured to output the real-time pose of the carrier, project the LiDAR point cloud onto the world coordinate system, and construct a global three-dimensional map.

[0049] Figure 3 This is a schematic diagram of the structure of a computing device provided in one embodiment of this specification. Figure 3 As shown, a computing device 300 includes a storage device 310 and a processor 320. The storage device 310 stores a computer program, and the processor 320 runs the computer program to enable the computing device 300 to perform the steps of the laser SLAM method with dynamic global attitude constraints.

[0050] Figure 4 This is a schematic diagram of the structure of a storage medium provided in one embodiment of this specification. Figure 4 As shown, a storage medium 400 stores a computer program 410 used in the computing device, which, when executed by a processor, implements the steps of the laser SLAM method with dynamic global attitude constraints.

[0051] In summary, the embodiments of this specification provide a laser SLAM method, system, device, and medium with dynamic global attitude constraints. This method can effectively suppress trajectory drift even in scenarios where GNSS signals are blocked or missing, improving the accuracy and robustness of positioning and mapping. It also significantly improves elevation accuracy and global consistency during long-term operation.

[0052] Those skilled in the art will understand that the accompanying drawings are merely schematic diagrams of one embodiment, and the modules or processes shown in the drawings are not necessarily essential for implementing the present invention.

[0053] Those skilled in the art will understand that the modules in the apparatus of the embodiments can be distributed in the apparatus of the embodiments as described in the embodiments, or they can be located in one or more devices different from this embodiment with corresponding changes. The modules of the above embodiments can be combined into one module, or they can be further divided into multiple sub-modules.

[0054] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A laser SLAM method with dynamic global attitude constraints, characterized in that, include: Acquire point cloud data from lidar, angular velocity and acceleration data from inertial measurement unit (IMU), and position data from global navigation satellite system (GNSS). Perform time synchronization and noise reduction on all sensor data, and perform motion distortion correction on point cloud data. Construct LiDAR matching factors using corrected point cloud data; Pre-integrate the IMU data between adjacent key data frames to construct the IMU pre-integration factor; calculate the pitch angle and roll angle observations of the carrier in a static or uniform motion state based on acceleration data to construct the dynamic attitude constraint factor. A tightly coupled graph optimization framework based on factor graphs is constructed, modeling the LiDAR matching factor, IMU pre-integration factor, and dynamic attitude constraint factor as constraints of the factor graph. The iSAM2 algorithm is used to incrementally optimize the factor graph, outputting the real-time pose of the carrier. The objective function of the tightly coupled graph optimization framework is: in, , The residuals of the LiDAR matching factor. The residuals of the IMU pre-integration factor, The residuals of the GNSS position factors are calculated from GNSS data. The residual of the dynamic attitude constraint factor. The corresponding covariance matrix; during the solution process, the online calibration state vector includes gyroscope zero drift offset and accelerometer zero drift offset. According to the state of the GNSS signal, the weights of the dynamic attitude constraint factors in the factor diagram are adjusted according to the preset rules. The step of adjusting the weights of the dynamic attitude constraint factors in the factor graph according to the state of the GNSS signal and a preset rule includes: Obtain the standard deviation of GNSS position measurements Set the baseline covariance and standard deviation threshold ; When GNSS signals are available, calculate the covariance of the current dynamic attitude constraint factor according to the formula. in, The minimum covariance of the attitude constraints corresponds to the maximum weight, set as follows: , The standard deviation of GNSS position measurements. The maximum standard deviation threshold available for GNSS is set to 0.1m. To adjust the coefficient, set it to 10; When GNSS signals are missing, the covariance of the dynamic attitude constraint factor will be... Set to minimum value ; The calculated covariance Time-domain smoothing filtering is performed using a first-order low-pass filter: in, The filter coefficient is set to 0.

2. The steps for online calibration of the state vector, including gyroscope zero drift offset and accelerometer zero drift offset, include: During factor graph optimization, the gyroscope zero drift offset and accelerometer zero drift offset of the IMU are added as state variables to be estimated into the optimization framework. Based on the constraints provided by the LiDAR matching factor, GNSS position factor, and dynamic attitude constraint factor, the estimated value of the IMU zero drift offset is updated. When the GNSS signal is available, the absolute position and velocity constraints of the GNSS are used to assist the convergence of the IMU zero drift offset; when the GNSS signal is missing, the IMU zero drift offset is observed using the LiDAR matching factor and dynamic attitude constraint factor. By projecting LiDAR point clouds onto the world coordinate system, a global 3D map is constructed.

2. The method according to claim 1, characterized in that, The step of constructing LiDAR matching factors using corrected point cloud data includes: Edge and planar features are extracted from LiDAR point cloud data. Feature points in the current frame are matched with feature points in the local map to obtain the LiDAR matching factor. The Voxelized Generalized Iterative Closest Point (VGICP) algorithm is used for point cloud matching, and the residual is defined as: in, For state vectors, and The first Frame and the A frame is a pose in the world coordinate system. The first result obtained by the VGICP algorithm Frame to the Relative pose transformation of frames, Let be the logarithmic mapping from Lie groups to Lie algebras.

3. The method according to claim 1, characterized in that, The step of pre-integrating IMU data between adjacent key data frames to construct the IMU pre-integration factor includes: The IMU pre-integration factor is obtained by pre-integrating the IMU data between two keyframes. The residual of the IMU pre-integration is defined as: in, , and These represent the relative rotation, relative velocity, and relative position obtained through IMU pre-integration, respectively. The time interval between two key data frames. Let m be the position (m) of the carrier in the world coordinate system. Let the velocity of the carrier in the world coordinate system be (m / s). Let be the rotation quaternion from the carrier coordinate system to the world coordinate system. This represents the gyroscope's zero-drift offset (rad / s). The accelerometer zero drift offset (m / s²) It is the acceleration due to gravity. Characterizes the transpose of a rotation matrix; By introducing Earth rotation compensation and Coriolis acceleration compensation, systematic integral errors in high-latitude regions or high-speed motion scenarios are corrected, including: Subtract the projected component of the Earth's rotational angular velocity in the IMU coordinate system from the angular velocity measurement. The vector of the Earth's rotational angular velocity in the local horizontal coordinate system is: in, The latitude of the location of the carrier. Let $\mathbf{a}$ be the Earth's rotational angular velocity, which is a constant value in the inertial coordinate system, with a magnitude of $\mathbf{a}$. The direction is along the Earth's rotation axis, pointing towards the North Pole; Subtract the projection of the Earth's rotational angular velocity in the carrier coordinate system from the gyroscope's measurement: in, This is the projection of the angular velocity of the vehicle coordinate system relative to the local horizontal coordinate system onto the vehicle coordinate system. This is the measurement value from the gyroscope. Let be the rotation matrix from the carrier coordinate system to the local horizontal coordinate system. This is the vector of the Earth's rotational angular velocity in the local horizontal coordinate system; Subtract the centripetal acceleration component and the Coriolis acceleration component caused by the Earth's rotation from the acceleration measurement. The centripetal acceleration caused by the Earth's rotation is: in, Let be the position vector of the carrier in the local horizontal coordinate system. This is the vector of the Earth's rotational angular velocity in the local horizontal coordinate system; The magnitude of the Coriolis acceleration is: in, Let the velocity of the carrier be in the local horizontal coordinate system. This is the vector of the Earth's rotational angular velocity in the local horizontal coordinate system; Subtract the projections of the Coriolis acceleration and centripetal acceleration in the carrier coordinate system from the accelerometer measurements: in, This represents the projection of the acceleration of the vehicle coordinate system relative to the local horizontal coordinate system onto the vehicle coordinate system. This is the measurement value from the accelerometer.

4. The method according to claim 1, characterized in that, The steps described above for calculating the pitch and roll angle observations of the carrier in a static or uniform motion state based on acceleration data, and constructing dynamic attitude constraint factors, include: When the carrier is at rest or in uniform linear motion, the specific force measured by the accelerometer Caused by gravitational acceleration, in, The gravitational acceleration vector in the world coordinate system. Characterizes the rotation matrix; Formulas for estimating pitch and roll angles: in, The pitch angle, For roll angle, , and These are the accelerometer readings on the three axes, respectively. To determine the effectiveness of the accelerometer attitude estimation, the magnitude of the vehicle's acceleration is calculated in real time. when When the accelerometer attitude estimation is deemed valid, then... The acceleration threshold is set to 0.1g; When the accelerometer attitude estimation is effective, a dynamic attitude constraint factor is constructed. The residual of the dynamic attitude constraint factor is defined as follows: in, and These are the pitch angle and roll angle in the state vector, respectively; The Jacobian matrix of the dynamic attitude constraint factor is: in, Let be the Jacobian matrix of the residuals in pairs of quaternions; A first-order low-pass filter is used to smooth the pitch and roll angles estimated by the accelerometer over time. in, These are the filter coefficients, with values ​​ranging from 0.1 to 0.

3.

5. A laser SLAM system with dynamic global attitude constraints according to any one of claims 1-4, characterized in that, It includes a data acquisition module, a factor extraction module, an optimization solution module, and a data output module, among which... The data acquisition module is configured to acquire point cloud data collected by lidar, angular velocity and acceleration data collected by inertial measurement unit (IMU), and position data collected by global navigation satellite system (GNSS), perform time synchronization and noise reduction on all sensor data, and perform motion distortion correction on point cloud data. The factor extraction module is configured to construct LiDAR matching factors using the corrected point cloud data; Pre-integrate the IMU data between adjacent key data frames to construct the IMU pre-integration factor; calculate the pitch angle and roll angle observations of the carrier in a static or uniform motion state based on acceleration data to construct the dynamic attitude constraint factor. The optimization solution module is configured to construct a tightly coupled graph optimization framework based on factor graphs, model the LiDAR matching factor, IMU pre-integration factor and dynamic attitude constraint factor as constraints of the factor graph, adjust the weight of the dynamic attitude constraint factor in the factor graph according to the state of the GNSS signal and according to preset rules, perform real-time optimization solution using the iSAM2 algorithm, and calibrate the IMU parameters online. The data output module is configured to output the real-time pose of the carrier, project the LiDAR point cloud onto the world coordinate system, and construct a global 3D map.

6. A computing device, characterized in that, The device includes a storage device and a processor, the storage device being used to store a computer program, and the processor running the computer program to cause the computing device to perform the steps of the method according to any one of claims 1-4.

7. A storage medium, characterized in that, It stores a computer program used in the computing device of claim 6, which, when executed by a processor, implements the steps of the method of any one of claims 1-4.

Images