Dynamic reinitialization method for external rotation type laser scanning system

Abstract

The invention provides a dynamic reinitialization method for an external rotation type laser scanning system, and belongs to the field of mobile surveying and mapping and robot autonomous positioning. According to the method, the motor encoder and the IMU are closely fused, motor-assisted distortion correction and motor-assisted pose initial value prediction are introduced, high-precision motor encoder data are used for providing strong geometric constraint and accurate initial value guidance for the laser odometer, the registration degradation risk under the low overlapping rate is effectively avoided, and the precision of the laser odometer is improved. The stability of front-end pose estimation is ensured; a dynamic weight mechanism based on the angular velocity of the base is designed, and the interference isolation problem in multi-source data fusion is effectively solved; and a step-by-step estimation-iterative correction strategy is adopted, so that optimization is prevented from falling into a local minimum value, and high-precision recovery of all parameters is realized. Through the design, the system state can be quickly recovered in the process of equipment movement and continuous scanning, the continuity of operation is ensured, and the adaptability and efficiency of the system in a complex dynamic environment are improved.

Description

Technical Field

[0001] This invention belongs to the field of mobile mapping and robot autonomous positioning technology, specifically relating to a dynamic reinitialization method for an externally rotating panoramic laser scanning system, which is particularly suitable for state recovery and parameter estimation of such systems under conditions of continuous rotating scanning and dynamic motion of the base. Background Technology

[0002] An externally rotating laser scanning system (Panoramic Scanning LiDAR System) is a sensing device capable of efficiently acquiring omnidirectional three-dimensional environmental information. The system mainly consists of a carrier base, a high-precision drive motor, and a laser ranging unit (multi-line lidar). Its core working mechanism involves rigidly mounting the laser ranging unit onto the motor rotor, using the motor to drive the laser ranging unit to continuously rotate at high speed around an external mechanical axis (usually perpendicular to the internal scanning plane of the lidar).

[0003] This unique mechanical structure endows the system with significant measurement advantages. Unlike traditional fixed LiDAR systems that rely on carrier movement to extend the scanning range, this system can cover omnidirectional space through its own mechanical rotation, even when the base is completely stationary, constructing a high-density panoramic 3D point cloud with an ultra-large field of view (360°×270°). This effectively overcomes the limitation of the narrow vertical field of view (typically only 30°~45°) of traditional multi-line LiDAR, enabling it to capture complete spatial structures such as ceilings and floors. It can be widely used in handheld mapping, UAV inspection, and mobile robot navigation. However, this special mechanical scanning structure and operating mode pose significant challenges to the stability and recovery capabilities of the Simultaneous Localization and Mapping (SLAM) system.

[0004] Existing laser inertial odometry methods, such as FAST-LIO (W. Xu and F. Zhang, "Fast-LIO: A fast, robust LiDAR-inertial odometry package by tightly-coupled iterated Kalman filter," IEEE Robot. Automat. Lett., 2021), typically assume the device is completely stationary during startup or initialization to use the mean values ​​from the inertial measurement unit (IMU) to separate the gravity vector and estimate sensor bias. However, if the system experiences tracking failure during motion, it often cannot automatically recover due to the inability to meet the stringent condition of being stationary, necessitating a forced shutdown and restart.

[0005] To address the initialization problem during motion, several dynamic initialization methods have emerged in recent years. J. Xu et al., "Dynamic Initialization for LiDAR-Inertial SLAM," IEEE / ASME Transactions on Mechatronics, 2025, proposed a D-LI-Init method, which achieves dynamic initialization by iteratively aligning the laser odometry with the IMU pre-integration. While this type of method has achieved some success in conventional handheld or vehicle-mounted scenarios, it still has significant limitations when facing the externally rotating panoramic scanning system described in this invention. Existing dynamic initialization methods such as D-LI-Init are highly dependent on the stability of the front-end laser odometry itself. However, externally rotating devices typically operate at high speeds (e.g., 180° / s), resulting in extremely low overlap rates of point clouds between adjacent frames and drastic changes in the field of view. Under these extreme conditions, the pure laser odometry, lacking external constraints, is prone to matching errors or drift, making it impossible to provide accurate relative pose constraints for initialization, thus causing the entire dynamic initialization process to fail. Therefore, there is an urgent need for a dynamic reinitialization method that can utilize the prior knowledge of motor kinematics to assist laser odometers in achieving robust and high-precision state recovery under conditions of high-dynamic rotation of the equipment and movement of the base. Summary of the Invention

[0006] This invention aims to solve the problem of difficulty in reinitializing laser inertial odometry under continuous rotation and dynamic motion conditions in external rotating systems. It provides a dynamic reinitialization method for external rotating laser scanning systems, wherein the external rotating laser scanning system includes a base, an inertial measurement unit (IMU), a lidar, and a motor driving the lidar to rotate; the method includes:

[0007] Step 1: Monitor the tracking status of the odometer system in real time. When the system is in normal tracking status, use IMU pre-integration to correct the distortion of the laser point cloud. When the system is detected to have failed to track or is in an uninitialized state, enter the dynamic reinitialization mode.

[0008] Step 2: In dynamic reinitialization mode, synchronously collect the angle data of laser point cloud and motor encoder, use the angle data of motor encoder to establish a kinematic model, assist the laser point cloud in distortion correction, and obtain the corrected laser point cloud;

[0009] Step 3: Based on the registration of the corrected laser point cloud and the local point cloud map, and combined with the changes in the motor encoder angle data, the pose trajectory of the laser odometer is estimated.

[0010] Step 4: Based on the solved pose change of the laser odometry and the rotation increment obtained by IMU pre-integration, construct the laser-IMU relative rotation residual; based on the projection of the rotation vector obtained by IMU pre-integration onto the motor rotation axis and the motor encoder angle increment, construct the motor-IMU mechanical motion constraint residual; introduce a dynamic weighting mechanism, fuse the above two residuals to construct a least squares optimization problem, and solve it to obtain the optimal gyroscope zero bias;

[0011] Step 5: Based on the gyroscope zero bias determined in Step 4, the laser odometry and IMU data are linearly aligned, the laser-IMU linear constraint equation is constructed, and the initial velocity vector and gravity vector are obtained by solving the equation. The gravity vector is then refined on the manifold space to obtain the refined gravity vector.

[0012] Step 6: Combine the obtained pose, gyroscope bias, accelerometer bias, initial velocity vector, and refined gravity vector as the initial values ​​of the system's global state; construct a factor graph optimization model that includes laser point cloud point-surface matching residuals and IMU pre-integration residuals, and iteratively correct all system parameters during subsequent tracking to complete the system's global state recovery.

[0013] Furthermore, in step 1, the odometry system performs tracking status monitoring as follows: The odometry system backend maintains a status monitor to calculate the root mean square error of matching the current laser frame with the local map point cloud in real time; if the root mean square error of matching for N consecutive frames is higher than a preset threshold, or if the odometry system variables are in an uninitialized state, the system tracking is determined to have failed, and it immediately switches to dynamic reinitialization mode; otherwise, the system tracking status is determined to be normal, and the point cloud is continued to be distorted using IMU pre-integration.

[0014] Furthermore, in step 2, the angle data from the motor encoder is used to assist in distortion correction of the laser point cloud, as detailed below:

[0015] Acquiring the k-th frame of laser scanning Laser point in laser coordinate system and the angle measured by the motor encoder ;

[0016] Using the kinematic model of the electric motor, the original laser point Projecting onto the base coordinate system, the correction point is obtained. :

[0017]

[0018] in, It is the real-time rotation matrix of the motor coordinate system relative to the base coordinate system, which is determined by the angle of the motor encoder; , These are the rotation and translation matrices of the lidar optical center relative to the center of the motor shaft, respectively, which are the external parameters of the lidar to the motor.

[0019] Subsequently, in order to adapt to the standard input interface of the subsequent laser odometry calculation method, the correction point in the base coordinate system was reverse-transformed to the start time of the current laser frame. In the laser coordinate system, the corrected point cloud is obtained. :

[0020]

[0021] in, This is the inverse rotation matrix of the motor coordinate system relative to the base coordinate system at the start of the k-th frame; through this transformation, all laser points are unified to the laser coordinate system at the same moment, eliminating the motion distortion caused by high-speed rotation.

[0022] Furthermore, in step 3, the pose trajectory of the laser odometry is obtained based on the following method:

[0023] The point cloud after correction in step 1 Projecting onto the corresponding plane of the local point cloud map, with the objective of minimizing the perpendicular distance from the point to the plane, we construct the point-to-surface residual and establish an optimization problem, specifically minimizing the error function:

[0024]

[0025] in, , Let represent the rotation matrix and translation vector of the laser odometry in the k-th frame relative to the local point cloud map, respectively, which are the pose information to be determined. This is the corrected point cloud for the k-th frame; Represented as the corresponding planar sampling point in the local point cloud map; This is the unit normal vector of the corresponding plane in the local point cloud map;

[0026] Based on the rigid kinematic model of the motor and lidar, the relative attitude change of the lidar is calculated using the motor encoder angle data corresponding to two adjacent laser frames. This value is then used as the initial value for the laser odometry pose estimation and substituted into the above optimization problem model, thereby improving the convergence speed and robustness of the laser odometry pose estimation.

[0027] Furthermore, in step 4, the laser-IMU relative rotation residual is constructed, as follows:

[0028] In the time interval between two adjacent frames of laser data Inside, the rotation increment is obtained by pre-integrating the IMU data. The rotation increment obtained by the motor-assisted laser odometer in step 3 is First, align the coordinate systems and set the rotation increment in the laser coordinate system. Transform to IMU coordinate system ,in The rotational extrinsic parameter from the IMU coordinate system to the lidar coordinate system is used; then, consistency constraints are applied with the rotational increment pre-integrated by the IMU to construct the residual as follows:

[0029]

[0030] in, For the logarithmic mapping of the Lie group SO(3);

[0031] Using the BCH approximation, the residual about Perform a first-order Taylor expansion, which takes the following form:

[0032]

[0033] in, This is the reference value (linearization point) for the gyroscope's zero bias. This is the update amount for the gyroscope's zero bias; , For SO(3) logarithmic mapping, the right Jacobian, This is the Jacobian matrix for the pre-integrated rotation increment with respect to the zero bias of the gyroscope.

[0034] Furthermore, in step 4, the motor-IMU mechanical motion constraint residuals are constructed, as follows:

[0035] The rotation vector is obtained by performing a logarithmic mapping on the rotation vector obtained after IMU pre-integration. ;

[0036] The rotation vector is directed towards the motor rotation axis. The projection, based on the mechanical principle of a motor rotating around a fixed X-axis, should correspond to the angle increment of the motor encoder to the lidar. Equal to each other, and based on this, construct the motor-IMU mechanical motion constraint residuals:

[0037]

[0038] Zero bias of the residual gyroscope Perform linearization:

[0039] .

[0040] Furthermore, in step 4, the objective function of the least squares optimization problem is as follows:

[0041]

[0042] in, and These are the information matrices (inverses of the covariance matrices) corresponding to the laser odometer constraint and the motor constraint, respectively, used to balance the weight ratio of different residuals in the optimization; This is a dynamic weighting factor that ensures the mechanical constraint weight is automatically reduced when the handheld device shakes violently (not in the direction of motor shaft rotation). Its calculation formula is as follows:

[0043]

[0044] in, Let be the angular velocity modulus of the base in the direction other than the rotation axis. This is the sensitivity coefficient.

[0045] Furthermore, in step 5, the process of solving for the initial velocity vector and gravity vector includes:

[0046] Pre-integral update: Based on the gyroscope zero bias calculated in step 4, the IMU pre-integral term between two adjacent frames is recalculated to obtain the rotation pre-integral quantity in the IMU coordinate system. Velocity pre-integral quantity and displacement pre-integral quantity For the high centrifugal force characteristics of the external rotating system, the accelerometer zero bias is achieved. With gravity vector Since direct decoupling is not possible, the accelerometer bias is temporarily set to zero in this stage. ;

[0047] Constructing the linear constraint equations for the laser-IMU: Based on the IMU kinematic model, establish the displacement recursive equations of the IMU in the world coordinate system W:

[0048]

[0049] in, , Let be the position vectors of the IMU in the world coordinate system for the k-th and k+1-th frames, respectively. Let be the velocity vector of the IMU in the world coordinate system in the k-th frame. The time interval between frame k and frame (k+1) is given. This is the gravity vector in the world coordinate system. Let be the rotation matrix from the IMU coordinate system to the world coordinate system in the k-th frame;

[0050] The displacement recursive equation is transformed to the IMU coordinate system of the k-th frame to eliminate the world-frame pose dependency. Based on the kinematic model, the difference between the actual displacement increment obtained from laser observation and the displacement increment obtained from IMU pre-integration is used to obtain the laser-IMU linear constraint equation:

[0051]

[0052] in, and The positions are estimated by the motor-assisted laser odometry in frames k and k+1, respectively. Let be the rotation matrix from the IMU coordinate system to the world coordinate system in the k-th frame. Let K be the IMU velocity vector corresponding to the laser in the k-th frame.

[0053] Optimization Solution: Constructing Single-Frame Observation Equations Based on Laser-IMU Linear Constraint Equations , where the state vector Stack all keyframes to construct an overdetermined system of equations. The least squares method is used to solve the problem. The initial velocity sequence is obtained. and gravity vector .

[0054] Furthermore, in step 5, the refined gravity vector is obtained based on the following method:

[0055] Define the tangent space correction model for the gravity vector:

[0056]

[0057] in, To optimize the gravity vector, To the initial gravity vector Orthogonal 3×2 tangent space basis, It is a two-dimensional correction value within the tangent space;

[0058] The magnitude of the gravity vector is equal to the magnitude of the actual gravitational acceleration. Construct the objective function as constraints. An iterative optimization algorithm is used to adjust the correction amount. The solution is performed to ensure that the magnitude is constrained to the standard gravitational acceleration while maintaining fine-tuning of the gravity direction. The refined gravity vector is obtained through iteration. .

[0059] Furthermore, in step 6, the iterative correction of all system parameters and the restoration of the system's global state are detailed as follows:

[0060] Define the global state vector of the odometer system This includes the IMU pose, velocity vector, gyroscope bias, accelerometer bias, and gravity vector in the world coordinate system for each frame within the sliding window; the pose information obtained in step 1, the gyroscope bias and accelerometer bias obtained in step 2 (at this time) are combined. The initial velocity sequence of the IMU obtained in step 3 and the refined gravity vector are used as the initial state values ​​of the system at the initial moment. ;

[0061] Within a defined sliding window, a factor graph optimization model is constructed, which includes IMU pre-integration residual factors and laser point cloud point-surface matching residual factors.

[0062] The IMU pre-integration residual factor is used to constrain the state of adjacent frames. Its residual form is the standard IMU pre-integration residual:

[0063]

[0064] in, The IMU pre-integration residual vector, , Let be the rotation matrix and position vector of the IMU in its own coordinate system for the k-th frame, respectively. Let be the velocity vector of the IMU in the k-th frame. The gravity vector in the IMU coordinate system. , , These are the IMU rotation pre-integration, velocity pre-integration, and displacement pre-integration between adjacent frames, respectively.

[0065] The laser point cloud point-surface matching residual factor: First, the k-th frame point cloud... Transform to world coordinate system to obtain Find the corresponding planar features (sampling points of the plane) in the local point cloud map. Plane unit normal vector The residuals are constructed as follows:

[0066]

[0067] Construct the total residual objective function for factor graph optimization:

[0068]

[0069] in, The information matrix of the IMU pre-integrated residuals. The information matrix for matching the laser point cloud point surface residuals is used to balance the weight ratio of the two types of residuals in the optimization process.

[0070] The total residual objective function is solved iteratively. During the iterative optimization process, the accelerometer zero bias is... It will gradually converge from 0 to the true value, and at the same time, the system's global state vector The remaining parameters, including gyroscope bias, pose in each frame, velocity vector, and gravity vector, were also optimized and corrected synchronously, ultimately restoring the state of the entire system.

[0071] The beneficial effects of this invention are:

[0072] 1. Robust reinitialization under high-dynamic rotation conditions: Existing laser inertial odometry typically requires the device to be completely stationary to complete parameter initialization. This is extremely difficult to meet in scenarios requiring continuous operation, such as UAV inspection and handheld mobile mapping, especially in emergency situations where odometry tracking fails and dynamic reinitialization is urgently needed. This invention, through the tight integration of the motor encoder and IMU, can quickly restore the system state during device movement and continuous scanning without stopping device rotation or keeping the device base stationary. This not only ensures the continuity of operation but also greatly improves the system's adaptability and operational efficiency in complex dynamic environments.

[0073] 2. Overcomes the problem of odometer trajectory drift caused by drastic changes in the field of view in external rotation systems: In response to the problem that the high-speed rotation of external rotating lidar results in extremely low overlap of point clouds between consecutive frames and that traditional laser odometers are prone to trajectory drift, this invention innovatively introduces "motor-assisted distortion correction" and "motor-assisted initial pose prediction". It uses high-precision motor encoder data to provide strong geometric constraints and accurate initial value guidance for the laser odometer, effectively avoiding the risk of registration degradation under low overlap rate, ensuring the stability of front-end pose estimation, and providing a reliable benchmark for subsequent parameter calculation.

[0074] 3. Significantly improved anti-interference capability in complex motion scenarios such as handheld devices: This invention designs a dynamic weighting mechanism based on the base angular velocity, effectively solving the interference isolation problem in multi-source data fusion. During reinitialization, when a violent shaking of the base (such as the handheld device, vehicle-mounted device, or drone device) is detected, the algorithm can automatically reduce the weight of the motor's mechanical constraints. This mechanism successfully decouples the motor's own rotational motion from the external disturbance motion of the base, avoiding the impact of external irregular motion on the accuracy of gyroscope zero-bias estimation, thereby ensuring the robustness of the system on non-stationary carriers.

[0075] 4. Solved the problem of unobservable accelerometer bias under high centrifugal force interference: Addressing the issue that the high centrifugal force generated by the external rotating structure makes it difficult to decouple the accelerometer bias from gravity (i.e., unobservable) within a short initialization window, this invention employs a "step-by-step estimation-iterative correction" strategy. First, ignoring the accelerometer bias, the velocity and gravity vectors are quickly analyzed using a linear model. Then, factor graph optimization is used to iteratively correct the accelerometer bias using long-term observation data. This coarse-to-fine strategy avoids optimization getting trapped in local minima and achieves high-precision recovery of all parameters. Attached Figure Description

[0076] Figure 1 This is a schematic diagram of the dynamic re-initialization method of the present invention.

[0077] Figure 2 This is a schematic diagram of the hardware structure and coordinate system definition of an external rotating laser scanning system.

[0078] Figure 3 This is a schematic diagram of the factor graph optimization structure during the parameter iteration and correction stage.

[0079] Figure 4 These are comparison images of the point cloud distortion correction effect assisted by the motor in the embodiments of the present invention; where (a) is the correction of the uniform motion model and (b) is the correction assisted by the motor.

[0080] Figure 5 This is a comparison diagram of the trajectory recovery effect before and after dynamic re-initialization in an embodiment of the present invention. Detailed Implementation

[0081] The specific embodiments of the present invention will be further described in detail below with reference to the accompanying drawings and examples. The embodiments of the present invention are implemented based on the technical solution of the present invention, and detailed implementation methods and specific operating procedures are given. However, the scope of protection of the present invention is not limited to the following embodiments.

[0082] like Figure 2 As shown, the external rotation laser scanning system used in this embodiment includes the following core components and coordinate system definitions: defining the base coordinate system. The origin is located at the center of the motor stator. The motor is mounted on a base, and the motor coordinate system... The origin of the base coordinate system coincides with the origin of the base coordinate system, and the X-axis of both systems is aligned. The motor rotor drives the lidar to rotate around the X-axis (i.e., the roll axis) of the motor, and the motor encoder outputs the rotation angle in real time. A LiDAR (Light Detection and Ranging) system is rigidly connected to the motor rotor, defining a laser coordinate system. The external parameters of the LiDAR's optical center relative to the motor shaft center are calibrated as follows: ,in , The rotation and translation matrices of the extrinsic parameters are given respectively. The Inertial Measurement Unit (IMU) is a built-in module of the lidar system, consisting of an accelerometer and a gyroscope. The IMU coordinate system is defined. The extrinsic parameter calibration of the IMU and lidar is as follows: ,in , Let represent the rotation and translation matrices of the IMU relative to the lidar, respectively. The world coordinate system is defined as follows: The world coordinate system attitude is consistent with the attitude of the first frame of the lidar, and the world coordinate system origin is... It coincides with the origin of the coordinate system of the first frame lidar.

[0083] like Figure 1 As shown, this embodiment of the invention provides a dynamic re-initialization method for an external rotating laser scanning system, specifically including the following steps:

[0084] Step 1: Odometer tracking status monitoring and motor-assisted point cloud distortion correction

[0085] Step 1.1, Odometer Tracking Status Monitoring

[0086] The laser-inertial odometry (LIO) system maintains a state monitor at the back end to calculate the root mean square error (RMSE) of matching the current laser frame with the local map point cloud in real time; if the RMSE of matching for N consecutive frames (N=3) exceeds a preset threshold... If the odometry system variables are in an uninitialized state (set to 0.5m), the system tracking is considered to have failed and enters the dynamic reinitialization mode; otherwise, the odometry system tracking status is normal and the point cloud is still being distorted using IMU pre-integration.

[0087] Step 1.2, Motor-assisted point cloud distortion correction

[0088] In dynamic reinitialization mode, the IMU parameters are unknown, making it unsuitable for laser point cloud correction; while using only a uniform motion model cannot meet the correction requirements for high-speed rotational motion. To obtain an accurate geometrically rigid point cloud, this embodiment utilizes high-frequency angle feedback from the motor encoder. To construct a motion model and remove distortion from the point cloud, the following steps are taken:

[0089] Suppose that during the k-th frame of laser scanning, a point in the laser coordinate system... The sampling time is At that moment, the motor encoder reading was Construct the real-time rotation matrix of the motor coordinate system relative to the base coordinate system. Because the motor rotates around the X-axis, the rotation matrix for:

[0090]

[0091] Then the original laser point Correction points projected onto the base coordinate system The calculation formula is:

[0092]

[0093] This step unifies all laser points within a frame to the base coordinate system at the start of that frame, completely eliminating motion distortion caused by the high-speed rotation of the motor. The point cloud distortion removal effect is as follows: Figure 4 As shown.

[0094] To facilitate subsequent pose calculation and data matching, the point cloud data in the base coordinate system is further reverse-transformed to the start time of the current laser frame. In the laser coordinate system, the corrected point cloud is obtained. :

[0095]

[0096] in, The inverse rotation matrix of the motor coordinate system relative to the base coordinate system at the start of frame k (corresponding to the motor angle) The inverse transformation yields the undistorted point cloud data in the laser coordinate system, providing a unified benchmark for subsequent laser odometry pose estimation.

[0097] Step 1.3, Motor-assisted estimation of the pose information of the laser odometry.

[0098] The actual rotation angle of the motor is calculated based on the encoder data corresponding to two adjacent frames of laser data. This is then used to calculate the relative attitude change of the lidar, which is used as the initial value for the laser odometry pose estimation. The pose-transformed point cloud is projected onto the corresponding plane of the local point cloud map. With the goal of minimizing the vertical distance from the point to the plane, a point-to-plane residual is constructed, and an optimization problem is established, specifically minimizing the error function. ,in, , Let these represent the rotation matrix and translation vector of the laser odometry in the k-th frame relative to the local point cloud map, respectively. The point cloud of the k-th frame after correction. Represented as the corresponding planar sampling point in the local point cloud map. This is the unit normal vector of the corresponding plane in the local point cloud map.

[0099] By solving the point-surface residual optimization problem frame by frame, the continuous pose trajectory of the laser odometry is obtained, providing a stable and accurate observation benchmark for subsequent steps.

[0100] Step 2, Gyroscope bias estimation based on dynamic weight constraints

[0101] This step integrates two types of constraints—"laser-IMU relative rotation" and "motor mechanical motion"—to construct a joint optimization problem, thereby estimating the gyroscope's zero bias. .

[0102] Step 2.1, Construct constrained residuals

[0103] First, construct the laser-IMU relative rotation residual. The process is as follows:

[0104] Within the time interval of two adjacent frames of laser data, the rotation increment is obtained by pre-integrating the IMU data. The rotation increment obtained by the motor-assisted laser odometer is Because IMU and lidar have extrinsic parameters (The rotational extrinsic parameters of the laser radar to the IMU) need to be transformed from the laser coordinate system to the IMU coordinate system, and then consistent with the rotational increments pre-integrated by the IMU. The residuals are constructed as follows:

[0105]

[0106] in, The logarithmic mapping of the Lie group SO(3) is used; the residual is approximated by BCH. about Perform a first-order Taylor expansion, which takes the following form:

[0107]

[0108] in, This is the reference value for zero bias of the gyroscope. This is the update amount for the gyroscope's zero bias; ,in, For SO(3) logarithmic mapping, the right Jacobian, Let Jacobian matrix be the pre-integrated rotation increment with respect to zero bias of the gyroscope. Rotational extrinsic parameters from IMU to lidar ( ).

[0109] Then, construct the motor-IMU mechanical motion constraint residuals. The process is as follows:

[0110] The rotation vector is obtained by performing a logarithmic mapping on the rotation vector obtained after IMU pre-integration. ; direct the rotation vector towards the motor's rotation axis The projection, based on the mechanical principle of a motor rotating around a fixed X-axis, should correspond to the angle increment of the motor encoder to the lidar. Equal to each other, and based on this, construct the motor-IMU mechanical motion constraint residuals:

[0111]

[0112] Similarly, zero bias is applied to this part of the residual gyroscope. After linearization, we obtain a first-order approximate expression:

[0113]

[0114] Step 2.2: Calculate the dynamic weighting factor and solve for the gyroscope zero bias.

[0115] A least-squares optimization problem is constructed by fusing the relative rotation residual of the laser-IMU and the mechanical motion constraint residual of the motor-IMU. The Gauss-Newton method is then used to solve the problem, yielding the zero bias of the gyroscope. The optimal solution, with the following objective function:

[0116]

[0117] in, and These are information matrices corresponding to laser odometer constraints and motor constraints, used to balance the weight ratio of different residuals in the optimization; and to address the impact of base non-rotation axis sway on motor constraints in handheld and vehicle-mounted application scenarios. To address the interference caused, this invention introduces a dynamic weighting mechanism, defining the dynamic weighting factors as follows:

[0118]

[0119] in, Let be the angular velocity modulus of the base in the non-rotational axis directions (Y-axis and Z-axis). This represents the algorithm's sensitivity coefficient. When the base shakes violently, the rotation observed by the laser includes not only the rotation driven by the motor but also the disturbance of the base itself. At this time, the dynamic weighting factor will automatically tend to 0, thereby weakening or even shielding the motor constraint in the optimization process and improving the system's robustness to external motion disturbances.

[0120] Step 3, Initial velocity vector and gravity vector estimation

[0121] This step constructs linear observation constraints based on the IMU kinematic model, aligns the laser odometry with the IMU data, thereby obtaining the initial velocity and gravity vector of the odometry system. At the same time, the magnitude of the gravity vector is refined, and the output accurate initial kinematic values ​​will provide basic support for subsequent global pose optimization of the system.

[0122] Step 3.1, Construction of Laser-IMU Linear Constraints

[0123] Based on the gyroscope zero bias obtained in step 2.2 The IMU pre-integration term is updated between two adjacent frames (frame k and frame k+1) to obtain the rotational pre-integration in the IMU coordinate system. Velocity pre-integral quantity and displacement pre-integral quantity For the high centrifugal force characteristics of the external rotating system, the accelerometer zero bias is achieved. With gravity vector Since direct decoupling is not possible, the accelerometer is set to zero bias in this step. This eliminates its interference with subsequent solutions.

[0124] For two adjacent frames of laser data output by the laser odometry (frames k and k+1), based on the IMU kinematic model, a recursive equation for the IMU position in the world coordinate system W is established:

[0125]

[0126] in, , Let be the position vectors of the IMU in the world coordinate system for the k-th and k+1-th frames, respectively. Let be the velocity vector of the IMU in the world coordinate system in the k-th frame. The time interval between frame k and frame (k+1) is given. This is the gravity vector in the world coordinate system. Let be the rotation matrix from the IMU coordinate system to the world coordinate system in the k-th frame.

[0127] Multiply both sides of the above position recursive equation by the left-hand side using the rotation matrix from the IMU coordinate system to the world coordinate system in the k-th frame, and then transpose the equation. By transforming all variables in the world coordinate system to the IMU coordinate system of the k-th frame, the dependency of the equation on the world coordinate system pose is eliminated, and the laser-IMU linear constraint equation is obtained after simplification:

[0128]

[0129] in, and These are the position estimates obtained from the motor-assisted laser odometry in frames k and k+1, respectively. Let be the IMU velocity vector corresponding to the laser in the k-th frame. The left side of the equation... In practice, the actual displacement increment of the IMU is calculated using laser observation, and the difference between this and the nominal displacement increment obtained from pre-integration is used as the observation for the constraint equations.

[0130] Step 3.2, Construction of observation equations and solution of initial values

[0131] Based on the laser-IMU linear constraint equations obtained in step 3.1, the system observation equations within a single frame observation interval are constructed:

[0132]

[0133] Among them, the observation vector coefficient matrix State vector It includes the velocity vector in the IMU coordinate system and the gravity vector in the world coordinate system of the k-th frame to be solved.

[0134] For all keyframes output by the laser odometry, construct the single-frame observation equations one by one in the manner described above and stack them to obtain a global overdetermined linear equation system. ,in For global stacked observation vectors, This is the global stacked coefficient matrix. The least squares method is used to solve the globally overdetermined linear equation system, and the solution formula is:

[0135]

[0136] in, for The inverse matrix, Global stacking coefficient matrix The transpose; from the solution results Decoupling yields the initial velocity sequence in the IMU coordinate system Initial gravity vector in world coordinate system .

[0137] Step 3.3, Gravity Vector Optimization

[0138] The initial gravity vector obtained in step 3.2 through least squares solution Without applying a modulus constraint, its modulus is similar to the actual gravitational acceleration modulus ( There is a deviation, therefore, it is fine-tuned within the tangent space of the gravity vector. The tangent space correction model for the gravity vector is defined as follows:

[0139]

[0140] in, To optimize the gravity vector, To the initial gravity vector Orthogonal 3×2 tangent space basis (by The orthogonal complement space is obtained by solving for the direction of the gravity vector in SO(3) space, which is the legal correction direction of the gravity vector. It is a two-dimensional correction value in the tangent space.

[0141] The magnitude of the gravity vector is equal to the magnitude of the actual gravitational acceleration. Construct the objective function as constraints. An iterative optimization algorithm is used to adjust the correction amount. The solution is performed, and each iteration is based on the tangent space basis. The gravity vector is fine-tuned within a defined legal direction to prevent deviations in the correction direction. The refined gravity vector is obtained through iteration. Used for updating This provides accurate initial kinematic values ​​for subsequent system optimization.

[0142] Step 4: Parameter Iteration and System State Recovery

[0143] This step takes the gyroscope zero bias, initial velocity sequence, and refined gravity vector obtained in the previous steps as initial values ​​and inputs them into the laser-inertial odometry (LIO) system. By constructing a factor graph optimization model, the system parameters are iteratively corrected, and the global state such as system pose and velocity is accurately restored, ensuring the continuous and stable operation of the odometry system.

[0144] Step 4.1, Factor graph optimization model construction

[0145] Define the global state vector of the odometer system This includes the IMU pose, velocity vector, gyroscope bias, accelerometer bias, and gravity vector in the world coordinate system for each frame within the sliding window. The pose obtained in step 1, the gyroscope bias obtained in step 2, and the accelerometer bias obtained at this point... The initial velocity sequence of the IMU and the refined gravity vector obtained in step 3 are used as the initial state values ​​of the system at the initial moment. .

[0146] Within a defined sliding window, a factor graph optimization model is constructed. This model includes two core constraint factors: the IMU pre-integration residual factor and the laser point cloud point-area matching residual factor. This achieves tight coupling constraints between the IMU and the LiDAR data (e.g., ...). Figure 3 (As shown).

[0147] Construct an IMU pre-integration residual factor, which is used to constrain the state of two adjacent frames within a sliding window. Its residual form is the standard IMU pre-integration residual:

[0148]

[0149] in, The IMU pre-integration residual vector, , Let be the rotation matrix and position vector of the IMU in its own coordinate system for the k-th frame, respectively. Let be the velocity vector of the IMU in the k-th frame. The gravity vector in the IMU coordinate system. , , These are the IMU rotation pre-integration, velocity pre-integration, and displacement pre-integration between adjacent frames, respectively. This represents the time interval between two adjacent frames.

[0150] To construct the laser point cloud point-to-surface matching residual factor, the point cloud data of the k-th frame of the LiDAR is first processed using the pose parameters in the system's global state vector. Transformation from laser coordinate system to world coordinate system Then, in the local point cloud map under the world coordinate system, for Find the corresponding planar features and obtain the sampling points of that plane. With the unit normal vector of the plane Define the laser point cloud point-to-plane matching residual as the perpendicular distance from the point to the plane:

[0151]

[0152] Step 4.2, Parameter Iterative Optimization and System Global State Recovery

[0153] Based on the two types of core residual factors constructed in step 4.1, the total residual objective function for factor graph optimization is constructed as follows:

[0154]

[0155] in, The information matrix of the IMU pre-integrated residuals. The information matrix for matching laser point cloud points and surfaces is used to balance the weight ratio of the two types of residuals in the optimization process.

[0156] The Levenberg-Marquardt algorithm is used to iteratively solve the total residual objective function. During the iterative optimization process, the system's global state vector... All parameters, including accelerometer bias, gyroscope bias, pose in each frame, velocity vector, and gravity vector, are synchronously optimized and corrected during the iteration process, achieving fine re-initialization of all parameters of the tightly coupled LIO system; simultaneously, based on the optimized global state vector... It extracts the pose, velocity and other information of each frame within the sliding window to complete the accurate restoration of the system's global state.

[0157] like Figure 5 As shown, the effect of this re-initialization process on correcting the system trajectory is clearly demonstrated. Without the dynamic re-initialization method of this invention, the system trajectory would rapidly diverge over time and produce irreversible severe drift due to the lack of effective initial constraints. After applying the method described in this invention, the system can quickly correct state deviations using motor-assisted observation constraints, re-converging the divergent trajectory to the accurate path, thereby rapidly restoring normal system operation without stopping equipment rotation.

[0158] The above description represents a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A dynamic reinitialization method for an external rotating laser scanning system, the external rotating laser scanning system comprising a base, an IMU, a lidar, and a motor for driving the lidar to rotate; characterized in that, include: Step 1: Monitor the tracking status of the odometer system in real time. When the system is in normal tracking status, use IMU pre-integration to correct the distortion of the laser point cloud. When the system is detected to have failed to track or is in an uninitialized state, enter the dynamic reinitialization mode. Step 2: In dynamic reinitialization mode, synchronously collect the angle data of laser point cloud and motor encoder, use the angle data of motor encoder to establish a kinematic model, assist the laser point cloud in distortion correction, and obtain the corrected laser point cloud; Step 3: Based on the registration of the corrected laser point cloud and the local point cloud map, and combined with the changes in the motor encoder angle data, the pose trajectory of the laser odometer is estimated. Step 4: Based on the solved pose change of the laser odometry and the rotation increment obtained by IMU pre-integration, construct the laser-IMU relative rotation residual; based on the projection of the rotation vector obtained by IMU pre-integration onto the motor rotation axis and the motor encoder angle increment, construct the motor-IMU mechanical motion constraint residual; introduce a dynamic weighting mechanism, fuse the above two residuals to construct a least squares optimization problem, and solve it to obtain the optimal gyroscope zero bias; Step 5: Based on the gyroscope zero bias determined in Step 4, the laser odometry and IMU data are linearly aligned, the laser-IMU linear constraint equation is constructed, and the initial velocity vector and gravity vector are obtained by solving the equation. The gravity vector is then refined on the manifold space to obtain the refined gravity vector. Step 6: Combine the obtained pose, gyroscope bias, accelerometer bias, initial velocity vector, and refined gravity vector as the initial values ​​of the system's global state; construct a factor graph optimization model that includes laser point cloud point-surface matching residuals and IMU pre-integration residuals, and iteratively correct all system parameters during subsequent tracking to complete the system's global state recovery.

2. The dynamic re-initialization method for an external rotating laser scanning system according to claim 1, characterized in that, In step 1, the backend of the odometer system maintains a status monitor to calculate the root mean square error of the matching between the current laser frame and the local map point cloud in real time. If the root mean square error of the matching of N consecutive frames is higher than the preset threshold, or if the odometer system variables are in an uninitialized state, the system tracking is determined to have failed, and the system immediately switches to dynamic reinitialization mode. Otherwise, the system tracking status is determined to be normal, and the IMU pre-integration is continued to be used to correct distortion of the point cloud.

3. A dynamic re-initialization method for an external rotating laser scanning system according to claim 1 or 2, characterized in that, In step 2, the angle data from the motor encoder is used to assist in distortion correction of the laser point cloud, as detailed below: Acquiring the k-th frame of laser scanning Laser point in laser coordinate system and the angle measured by the motor encoder ; Using the kinematic model of the electric motor, the original laser point Projecting onto the base coordinate system, the correction point is obtained. : in, It is the real-time rotation matrix of the motor coordinate system relative to the base coordinate system, which is determined by the angle of the motor encoder; , These are the rotation and translation matrices of the lidar optical center relative to the center of the motor shaft, respectively, which are the external parameters of the lidar to the motor. The correction point in the base coordinate system is reverse-transformed to the start time of the current laser frame. In the laser coordinate system, the corrected point cloud is obtained. : in, is the inverse rotation matrix of the motor coordinate system relative to the base coordinate system at the start of frame k.

4. The dynamic re-initialization method for an external rotating laser scanning system according to claim 3, characterized in that, In step 3, the pose trajectory of the laser odometry is obtained based on the following method: The point cloud after correction in step 1 Projecting onto the corresponding plane of the local point cloud map, with the objective of minimizing the perpendicular distance from the point to the plane, we construct the point-to-surface residual and establish an optimization problem, specifically minimizing the error function: in, , Let represent the rotation matrix and translation vector of the laser odometry in the k-th frame relative to the local point cloud map, respectively, which are the pose information to be determined. This is the corrected point cloud for the k-th frame; Represented as the corresponding planar sampling point in the local point cloud map; This is the unit normal vector of the corresponding plane in the local point cloud map; By using the motor encoder angle data corresponding to two adjacent laser frames, the relative attitude change of the lidar is calculated. The relative attitude change of the lidar is then used as the initial value for the laser odometry pose estimation and substituted into the above optimization problem model to improve the convergence speed and robustness of the laser odometry pose estimation.

5. A dynamic re-initialization method for an external rotating laser scanning system according to claim 4, characterized in that, In step 4, the laser-IMU relative rotation residual is constructed, as follows: In the time interval between two adjacent frames of laser data Inside, the rotation increment is obtained by pre-integrating the IMU data. The rotation increment obtained by the motor-assisted laser odometer in step 3 is ; Rotation increment in laser coordinate system Transform to the IMU coordinate system, and then use the rotation increment pre-integrated by the IMU. Apply consistency constraints and construct the residuals as follows: in, For the logarithmic mapping of the Lie group SO(3); The external parameter for rotation from the IMU coordinate system to the lidar coordinate system; Using the BCH approximation, the residual about Perform a first-order Taylor expansion, which takes the following form: in, This is the reference value for zero bias of the gyroscope. This is the update amount for the gyroscope's zero bias; , For SO(3) logarithmic mapping, the right Jacobian, This is the Jacobian matrix for the pre-integrated rotation increment with respect to the zero bias of the gyroscope.

6. A dynamic re-initialization method for an external rotating laser scanning system according to claim 5, characterized in that, In step 4, the motor-IMU mechanical motion constraint residuals are constructed, as follows: The rotation vector is obtained by performing a logarithmic mapping on the rotation vector obtained after IMU pre-integration. ; The rotation vector is directed towards the motor rotation axis. The projection, based on the mechanical principle of a motor rotating around a fixed X-axis, should correspond to the angle increment of the motor encoder to the lidar. Equal to each other, and based on this, construct the motor-IMU mechanical motion constraint residuals: Zero bias of the residual gyroscope Perform linearization: .

7. A dynamic re-initialization method for an external rotating laser scanning system according to claim 6, characterized in that, In step 4, the objective function of the least squares optimization problem is as follows: in, and These are the information matrices corresponding to laser odometer constraints and motor constraints, respectively. The dynamic weighting factor is calculated using the following formula: in, Let be the angular velocity modulus of the base in the direction other than the rotation axis. This is the sensitivity coefficient.

8. A dynamic re-initialization method for an external rotating laser scanning system according to claim 7, characterized in that, Step 5, the process of solving for the initial velocity vector and gravity vector includes: Pre-integral update: Based on the gyroscope zero bias calculated in step 4, the IMU pre-integral term between two adjacent frames is recalculated to obtain the rotation pre-integral quantity in the IMU coordinate system. Velocity pre-integral quantity and displacement pre-integral quantity For the high centrifugal force characteristics of the external rotating system, the accelerometer zero bias is achieved. With gravity vector Since direct decoupling is not possible, the accelerometer bias is temporarily set to zero in this stage. ; Constructing the linear constraint equations for the laser-IMU: Based on the IMU kinematic model, establish the displacement recursive equations of the IMU in the world coordinate system W: in, , Let be the position vectors of the IMU in the world coordinate system for the k-th and k+1-th frames, respectively. Let be the velocity vector of the IMU in the world coordinate system in the k-th frame. The time interval between frame k and frame (k+1) is given. This is the gravity vector in the world coordinate system. Let be the rotation matrix from the IMU coordinate system to the world coordinate system in the k-th frame; The displacement recursive equation is transformed to the IMU coordinate system of the k-th frame to eliminate the world-frame pose dependency. Based on the kinematic model, the difference between the actual displacement increment obtained from laser observation and the displacement increment obtained from IMU pre-integration is used to obtain the laser-IMU linear constraint equation: in, and The positions are estimated by the motor-assisted laser odometry in frames k and k+1, respectively. Let be the rotation matrix from the IMU coordinate system to the world coordinate system in the k-th frame. Let K be the IMU velocity vector corresponding to the laser in the k-th frame. Optimization Solution: Constructing Single-Frame Observation Equations Based on Laser-IMU Linear Constraint Equations , where the state vector Stack all keyframes to construct an overdetermined system of equations. The least squares method is used to solve the problem. The initial velocity sequence is obtained. and gravity vector .

9. A dynamic re-initialization method for an external rotating laser scanning system according to claim 8, characterized in that, In step 5, the refined gravity vector is obtained based on the following method: Define the tangent space correction model for the gravity vector: in, To optimize the gravity vector, To the initial gravity vector Orthogonal 3×2 tangent space basis, It is a two-dimensional correction value within the tangent space; The magnitude of the gravity vector is equal to the magnitude of the actual gravitational acceleration. Construct the objective function as constraints. An iterative optimization algorithm is used to adjust the correction amount. The solution is performed iteratively to obtain the refined gravity vector. .

10. A dynamic re-initialization method for an external rotating laser scanning system according to claim 9, characterized in that, Step 6 involves iterative correction of all system parameters and restoration of the system's global state, as detailed below: Define the global state vector of the odometer system This includes the IMU pose, velocity vector, gyroscope bias, accelerometer bias, and gravity vector in the world coordinate system for each frame within the sliding window; the pose information obtained in step 1, and the gyroscope bias and accelerometer bias obtained in step 2 are combined. The initial velocity sequence of the IMU obtained in step 3 and the refined gravity vector are used as the initial state values ​​of the system at the initial moment. ; Within a defined sliding window, a factor graph optimization model is constructed, which includes IMU pre-integration residual factors and laser point cloud point-surface matching residual factors. The IMU pre-integration residual factor is used to constrain the state of adjacent frames. Its residual form is the standard IMU pre-integration residual: in, The IMU pre-integration residual vector, , Let be the rotation matrix and position vector of the IMU in its own coordinate system for the k-th frame, respectively. Let be the velocity vector of the IMU in the k-th frame. This is the gravity vector in the IMU coordinate system; The laser point cloud point-surface matching residual factor: First, the k-th frame point cloud... Transform to world coordinate system to obtain Find the corresponding planar features in the local point cloud map to obtain the sampling points of that plane. and the unit normal vector of the plane The residuals are constructed as follows: Construct the total residual objective function for factor graph optimization: in, The information matrix of the IMU pre-integrated residuals. The information matrix for matching residuals to laser point cloud points and surfaces; The total residual objective function is solved iteratively. During the iterative optimization process, the accelerometer zero bias is... The system gradually converges from 0 to the true value, while the global state vector of the system... Other parameters, including gyroscope bias, pose of each frame, velocity vector, and gravity vector, were also optimized and corrected synchronously, ultimately restoring the state of the entire system.

Images