Laser slam height correction method and system fusing imu poses

Abstract

The application relates to a laser SLAM height correction method and system fusing IMU posture, and the method comprises the following steps: acquiring point cloud data of a laser radar, physical displacement data of a wheel speed meter and inertial measurement data; based on the point cloud data, a relative posture between frames including rotation and translation is calculated through feature matching to obtain observed Z-axis displacement; based on the inertial measurement data, a pitch angle is obtained; based on the pitch angle and the physical displacement data of the wheel speed meter, Z-axis expected displacement is obtained; based on the observed Z-axis displacement and the Z-axis expected displacement, kinematic consistency residuals are calculated, and whether correction is triggered is judged based on the kinematic consistency residuals; in response to triggering correction, the SLAM adds a unary constraint edge with relative displacement of zero in the Z-axis direction to the current posture node of the global posture graph, solves the global posture graph, takes the unary constraint edge as an anchor point, evenly distributes and eliminates accumulated Z-axis drift error, and outputs the corrected posture.

Description

Technical Field

[0001] This invention relates to the field of real-time positioning and mapping technology, and in particular to a laser SLAM height correction method and system that integrates IMU attitude. Background Technology

[0002] Laser-based SLAM technology is crucial for achieving environmental perception and localization in autonomous mobile robots and self-driving vehicles. It utilizes 3D point cloud information acquired by LiDAR (LiDAR radar) to estimate its own pose and simultaneously construct an environmental map through matching and optimization. However, many existing laser-based SLAM solutions, especially in scenarios with few structural features or flat terrain, exhibit weak constraints on vertical motion and are prone to slow but continuous drift along the Z-axis (the direction of gravity).

[0003] To address this issue, some zero-speed correction methods based on acceleration and angular velocity thresholds attempt to incorporate constraints into the front-end matching residuals of the laser odometry. However, these approaches suffer from a fundamental flaw: they mostly rely on acceleration calculated from laser data or simple wheel speed measurements to set fixed or adaptive speed thresholds. When a vehicle travels at a constant speed on a long, gentle slope, its acceleration and angular velocity fluctuations are minimal. Traditional algorithms are prone to misinterpreting this legitimate vertical motion on a slope as positioning drift on flat ground, forcing the Z-axis displacement back to zero and causing a complete positioning failure. This purely kinematic threshold judgment, lacking a real gravity reference, cannot adapt to the complex and ever-changing conditions of autonomous driving.

[0004] In summary, there is currently a lack of a laser SLAM height correction method and system to solve or partially solve the aforementioned problems. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of the existing technology by providing a laser SLAM height correction method and system that integrates IMU attitude, so as to solve or partially solve the problem that existing SLAM schemes do not fully consider the vertical drift problem in actual operation.

[0006] The objective of this invention can be achieved through the following technical solutions: One aspect of the present invention provides a laser SLAM height correction method incorporating IMU attitude, comprising the following steps: Acquire point cloud data from lidar, physical displacement data from wheel speedometers, and inertial measurement data; Based on the point cloud data, the inter-frame relative pose, including rotation and translation, is calculated through feature matching, and the observed Z-axis displacement is obtained. The pitch angle is obtained based on the inertial measurement data; The desired Z-axis displacement is obtained based on the pitch angle and the wheel speed gauge physical displacement data. Based on the observed Z-axis displacement and the expected Z-axis displacement, calculate the kinematic consistency residual, and determine whether to trigger a correction based on the kinematic consistency residual; In response to the trigger correction, SLAM adds a univariate constraint edge with zero relative displacement in the Z-axis direction to the current pose node of the global pose graph, solves the global pose graph, uses the univariate constraint edge as an anchor point, amortizes and eliminates the accumulated Z-axis drift error, and outputs the corrected pose.

[0007] As a preferred technical solution, after acquiring the point cloud data of the lidar, the physical displacement data of the wheel speed meter, and the inertial measurement data, the solution also includes timestamp alignment and using high-frequency odometer information to correct motion distortion of the point cloud within a scanning frame.

[0008] As a preferred technical solution, the desired Z-axis displacement is calculated using the following formula: in, The desired displacement along the Z-axis. For the physical displacement of the wheel speed gauge, The pitch angle.

[0009] As a preferred technical solution, the kinematic consistency residual is: in, For kinematic consistency residuals, The observed Z-axis displacement.

[0010] As a preferred technical solution, the process of determining whether to trigger correction based on the kinematic consistency residual includes: If the pitch angle remains stable, but there is an observed Z-axis displacement, the kinematic consistency residual will exceed the limit, i.e., there is vertical drift, triggering a correction; if the pitch angle is less than the threshold, it means that the laser displacement is consistent with the actual pitch motion of the vehicle, which is uphill or downhill, and no correction is made.

[0011] As a preferred technical solution, when the correction is triggered, the backend optimizer adds a univariate constraint edge with zero relative displacement in the Z-axis direction to the current pose node.

[0012] As a preferred technical solution, when the correction is triggered, a nonlinear optimizer is used to solve the global pose graph.

[0013] As a preferred technical solution, when adding the unary constraint edge, the weight of its constraint information matrix is ​​negatively correlated with the kinematic consistency residual.

[0014] Another aspect of the present invention provides a laser SLAM height correction system incorporating IMU attitude measurement, for implementing the aforementioned laser SLAM height correction method, the system comprising: The sensor unit is used to synchronously collect point cloud data, wheel speed meter physical displacement data, and inertial measurement data; The data preprocessing and front-end module is used to perform timestamps and coordinate system alignment on the data collected by the sensor unit, as well as point cloud motion distortion correction. It calculates the preliminary relative pose of the vehicle through inter-frame matching and obtains the observed Z-axis displacement. The Z-axis kinematic consistency verification module is used to obtain the pitch angle based on the inertial measurement data, obtain the expected Z-axis displacement based on the pitch angle and the wheel speed meter physical displacement data, calculate the kinematic consistency residual based on the observed Z-axis displacement and the expected Z-axis displacement, and determine whether to trigger correction based on the kinematic consistency residual. The backend optimization module based on the pose graph is used to respond to the trigger correction. SLAM adds a univariate constraint edge with zero relative displacement in the Z-axis direction to the current pose node of the global pose graph, solves the global pose graph, uses the univariate constraint edge as the anchor point, amortizes and eliminates the accumulated Z-axis drift error, and outputs the corrected pose.

[0015] As a preferred technical solution, the sensor unit includes a lidar, a wheel speedometer, and an inertial measurement unit.

[0016] Compared with the prior art, the present invention has at least one of the following beneficial effects: (1) Achieving height correction in SLAM: This invention introduces IMU attitude information as an absolute gravity reference, establishes a Z-axis kinematic consistency verification mechanism, and closely integrates it with the back-end pose graph optimization framework of SLAM, thereby effectively suppressing positioning drift in the height direction.

[0017] (2) Improve the numerical stability of pose graph optimization: By setting the information matrix weight of the unary constraint edge and the kinematic consistency residual of the Z-axis to be negatively correlated, the system can adaptively allocate optimization weights according to the observation confidence, effectively avoiding trajectory jumps caused by forced correction under complex working conditions.

[0018] (3) Precisely avoids incorrect corrections when driving on slopes: The theoretical expected displacement of the wheel speed meter coupled with the pitch angle of the IMU is introduced, which accurately distinguishes the actual uphill and downhill motion of the vehicle from the perspective of physical kinematics and overcomes the navigation crash problem that is easily caused by the traditional fixed threshold scheme.

[0019] (4) Enhance system robustness in GNSS denied environments: This solution relies solely on the wheel speedometer and inertial sensor data of the vehicle chassis to construct a height reference anchor point, without the need for external absolute positioning, thus ensuring high-precision three-dimensional positioning of the vehicle in environments where satellite signals are blocked, such as tunnels or underground garages. Attached Figure Description

[0020] Figure 1 This is a flowchart of the laser SLAM height correction method that integrates IMU attitude in the embodiment; Figure 2 This is a schematic diagram of SLAM height correction in the embodiment; Figure 3 This is a schematic diagram of a laser SLAM height correction system that integrates IMU attitude measurement in an embodiment. Detailed Implementation

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

[0022] Example 1 To address the problems existing in the prior art, this embodiment provides a laser SLAM height correction method that integrates IMU attitude. The aim is to establish a Z-axis kinematic consistency verification mechanism by introducing IMU attitude information as an absolute gravity reference, and to tightly integrate it with the back-end pose graph optimization framework of SLAM, thereby effectively suppressing positioning drift in the height direction.

[0023] See Figure 1 and Figure 2 The method includes the following steps: Step S1: Multi-source data acquisition and preprocessing. Data is acquired in parallel by lidar, wheel speedometer and MEMS-IMU. The preprocessing module completes timestamp alignment and uses high-frequency odometer information to correct motion distortion of point cloud within a frame scanning cycle.

[0024] In step S1, the acquisition and preprocessing of multi-source data specifically includes spatial synchronization and temporal synchronization. Spatial synchronization refers to the pre-calibration of the extrinsic parameter matrix between the lidar, wheel speedometer, and IMU. All sensor data is uniformly mapped to the IMU vehicle coordinate system. For time synchronization, the system uses the IMU's high-frequency hardware timestamp as a reference to perform linear interpolation alignment on the low-frequency LiDAR point cloud and wheel speed sensor data. Furthermore, when correcting point cloud motion distortion, the system uses the IMU's pre-integration within one LiDAR frame scan cycle to estimate its own high-frequency motion. Combined with the translational velocity provided by the wheel speed sensor, spherical linear interpolation is used to calculate the precise pose of each laser point at the time of emission. This allows the entire point cloud frame to be uniformly projected onto the coordinate system at the start of that frame, eliminating trailing distortion caused by vehicle motion.

[0025] Step S2: Front-end odometry calculation. The preprocessed point cloud data is sent to the front-end module, and the preliminary inter-frame relative pose, including rotation and translation, is calculated through feature matching. The observed Z-axis displacement is output. ; Step S3: Z-axis kinematic consistency check, this is a crucial step in determining whether a correction needs to be introduced. The system synchronously acquires the physical displacement of the wheel speed gauge. And the pitch angle calculated by the IMU .

[0026] In step S3, the pitch angle calculated by the IMU Instead of relying on simple integration of raw data, the system obtains the data through error-state Kalman filtering or an optimized IMU pre-integration mechanism. Specifically, the system uses the IMU's triaxial angular velocity and triaxial acceleration measurements to perform continuous-time integration under gravity. To eliminate the effects of gyroscope random walk and accelerometer bias in low-cost MEMS-IMUs, the system uses the high-frequency relative pose of the front-end odometer as the input filter for observation updates, estimating and compensating for the IMU's bias state in real time. The bias-compensated gravity vector is projected onto the current vehicle coordinate system, and the pitch angle, unaffected by high-frequency vibrations, is accurately extracted using inverse trigonometric functions. This ensures the physical reliability of the expected Z-axis displacement calculation.

[0027] Step S4: According to the formula Calculate the desired displacement and extract the kinematic consistency residuals. .

[0028] Step S5: This method abandons the traditional absolute velocity threshold judgment and instead evaluates this residual. If the pitch angle Maintain stability (flat ground), and Displacement occurred, resulting in Out of bounds is considered a vertical drift of the algorithm itself; if If the value is extremely small, it means that the laser displacement matches the vehicle's actual pitch motion, which is normal uphill or downhill motion and does not require correction.

[0029] Step S6: Backend Pose Graph Constraint Optimization. The SLAM backend continuously constructs the global pose graph. Upon receiving the drift confirmation signal from Step S5, the backend optimizer explicitly adds a unary edge with zero relative displacement in the Z-axis direction to the current pose node. Then, the backend starts a nonlinear optimizer (such as g2o or Ceres) to perform global optimization on the entire pose graph. During optimization, this Z-axis constraint acts as an absolute reference anchor point, effectively correcting the height estimates of all relevant pose nodes, thereby eliminating accumulated drift, outputting the corrected pose, completing the optimization calculation, and outputting a high-precision, height-drift-free global vehicle pose.

[0030] Preferably, when adding a univariate constraint edge in the Z-axis direction, the weight of its constraint information matrix is ​​dynamically changed, and the magnitude of this weight is related to the Z-axis kinematic consistency residual. There is a negative correlation; the smaller the residual and the more stable the IMU attitude, the greater the weight of the zero-displacement constraint edge.

[0031] Specifically, in the back-end nonlinear optimization stage of step S6, the optimization objective is to minimize the global cost function. It is composed of the front-end laser odometry residual, the IMU pre-integration residual, and the Z-axis univariate constraint residual proposed in this invention. The cost function can be expressed as: in, This represents the global pose node state. This represents the absolute displacement residual along the Z-axis of the current node. This is a robust kernel function (such as the Huber kernel or Cauchy kernel, used to suppress extreme outliers). The core is a univariate constraint information matrix (weights). Dynamic allocation is performed using an exponential decay model based on consistency residuals: in, The basic confidence weights set for the system This is the sensitivity adjustment coefficient. The kinematic consistency residuals calculated above. When The closer the constraint weights are to 0, the more likely the system is experiencing pure Z-axis divergence. Approaching the maximum value This forcefully flattens the pose; when the residual increases or the vehicle's attitude changes drastically, the weight decays exponentially, allowing the backend optimizer to transition smoothly and avoid global point cloud map tearing or trajectory discontinuity caused by forced constraints.

[0032] Example 2 Based on Example 1, this example provides a laser SLAM altitude correction system that integrates IMU attitude measurement. See [link to example]. Figure 3 It includes a sensor unit, a data preprocessing and front-end module, a Z-axis kinematic consistency verification module, and a pose graph-based back-end optimization module.

[0033] (1) Sensor unit.

[0034] It includes lidar, wheel speedometer, and inertial measurement unit (IMU), responsible for synchronously collecting environmental point cloud, vehicle displacement, and attitude angle information.

[0035] (2) Data preprocessing and front-end module.

[0036] The system receives sensor data, performs timestamp and coordinate system alignment, corrects point cloud motion distortion, and calculates the vehicle's preliminary relative pose using an inter-frame matching algorithm.

[0037] (3) Z-axis kinematic consistency test module.

[0038] As the core judgment unit, it is connected to the front-end module and the sensor unit respectively. The module uses the absolute gravity vector provided by the IMU to calculate the real-time vehicle pitch angle, and combines it with the physical mileage measured by the wheel speedometer to calculate the theoretical kinematic displacement of the vehicle in the Z-axis direction. By comparing the theoretical displacement with the Z-axis displacement calculated by the front-end module, it can accurately determine whether the system has experienced pure Z-axis positioning drift.

[0039] Specifically, the mathematical logic for the Z-axis kinematic consistency test module to determine positioning drift is as follows: Assume the wheel speed meter... The displacement measured within the time period is The current pitch angle calculated by the IMU is The Z-axis displacement calculated by the laser odometer front end is This module first calculates the theoretically expected Z-axis displacement. And calculate the kinematic consistency residuals along the Z-axis. When the residual If the error exceeds the set tolerance threshold and the IMU angular velocity variance is lower than the stationary threshold, it is determined that Z-axis positioning drift has occurred, triggering backend height correction.

[0040] (4) Backend optimization module based on pose graph.

[0041] When the verification module determines that Z-axis positioning drift has occurred, it adds a univariate constraint edge with zero Z-axis displacement to the current pose node in the back-end pose graph of SLAM, and forces the correction of cumulative height error during the global nonlinear optimization process.

[0042] Specifically, when the pose graph-based backend optimization module adds a univariate constraint edge in the Z-axis direction, the weight of its constraint information matrix dynamically changes; the magnitude of this weight is related to the Z-axis kinematic consistency residual. There is a negative correlation; the smaller the residual and the more stable the IMU attitude, the greater the weight of the zero-displacement constraint edge.

[0043] When the desired Z-axis displacement is calculated and All showed significant changes, and When the speed is less than the tolerance threshold, the system determines that the vehicle is in a real slope driving state. The consistency verification module will suppress the output of the zero speed confirmation signal to prevent erroneous height correction in normal uphill and downhill conditions.

[0044] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in the present invention, and these modifications or substitutions should all be covered within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A laser SLAM height correction method integrating IMU attitude, characterized in that, Includes the following steps: Acquire point cloud data from lidar, physical displacement data from wheel speedometers, and inertial measurement data; Based on the point cloud data, the inter-frame relative pose, including rotation and translation, is calculated through feature matching, and the observed Z-axis displacement is obtained. The pitch angle is obtained based on the inertial measurement data; The desired Z-axis displacement is obtained based on the pitch angle and the wheel speed gauge physical displacement data. Based on the observed Z-axis displacement and the expected Z-axis displacement, calculate the kinematic consistency residual, and determine whether to trigger a correction based on the kinematic consistency residual; In response to the trigger correction, SLAM adds a univariate constraint edge with zero relative displacement in the Z-axis direction to the current pose node of the global pose graph, solves the global pose graph, uses the univariate constraint edge as an anchor point, amortizes and eliminates the accumulated Z-axis drift error, and outputs the corrected pose.

2. The laser SLAM height correction method based on IMU attitude fusion according to claim 1, characterized in that, After acquiring point cloud data from the lidar, physical displacement data from the wheel speedometer, and inertial measurement data, the process also includes timestamp alignment and using high-frequency odometer information to correct motion distortion of the point cloud within a single scan frame.

3. The laser SLAM height correction method based on IMU attitude fusion according to claim 1, characterized in that, The desired Z-axis displacement is calculated using the following formula: in, The desired displacement along the Z-axis. For the physical displacement of the wheel speed gauge, The pitch angle.

4. The laser SLAM height correction method based on IMU attitude fusion according to claim 3, characterized in that, The kinematic consistency residual is: in, For kinematic consistency residuals, The observed Z-axis displacement.

5. The laser SLAM height correction method based on IMU attitude fusion according to claim 1, characterized in that, The process of determining whether to trigger correction based on the kinematic consistency residual includes: If the pitch angle remains stable, but there is an observed Z-axis displacement, the kinematic consistency residual will exceed the limit, i.e., there is vertical drift, triggering a correction; if the pitch angle is less than the threshold, it means that the laser displacement is consistent with the actual pitch motion of the vehicle, which is uphill or downhill, and no correction is made.

6. The laser SLAM height correction method based on IMU attitude fusion according to claim 1, characterized in that, When a correction is triggered, the backend optimizer adds a univariate constraint edge with zero relative displacement in the Z-axis direction to the current pose node.

7. The laser SLAM height correction method based on IMU attitude fusion according to claim 1, characterized in that, When a correction is triggered, a nonlinear optimizer is used to solve the global pose graph.

8. The laser SLAM height correction method based on IMU attitude fusion according to claim 1, characterized in that, When the unary constraint edge is added, the weight of its constraint information matrix is ​​negatively correlated with the kinematic consistency residual.

9. A laser SLAM altitude correction system integrating IMU attitude measurement, characterized in that, For implementing the laser SLAM height correction method as described in any one of claims 1-8, the system comprises: The sensor unit is used to synchronously collect point cloud data, wheel speed meter physical displacement data, and inertial measurement data; The data preprocessing and front-end module is used to perform timestamps and coordinate system alignment on the data collected by the sensor unit, as well as point cloud motion distortion correction. It calculates the preliminary relative pose of the vehicle through inter-frame matching and obtains the observed Z-axis displacement. The Z-axis kinematic consistency verification module is used to obtain the pitch angle based on the inertial measurement data, obtain the expected Z-axis displacement based on the pitch angle and the wheel speed meter physical displacement data, calculate the kinematic consistency residual based on the observed Z-axis displacement and the expected Z-axis displacement, and determine whether to trigger correction based on the kinematic consistency residual. The backend optimization module based on the pose graph is used to respond to the trigger correction. SLAM adds a univariate constraint edge with zero relative displacement in the Z-axis direction to the current pose node of the global pose graph, solves the global pose graph, uses the univariate constraint edge as the anchor point, amortizes and eliminates the accumulated Z-axis drift error, and outputs the corrected pose.

10. A laser SLAM altitude correction system integrating IMU attitude according to claim 9, characterized in that, The sensor unit includes a lidar, a wheel speedometer, and an inertial measurement unit.

Images