Method for realizing laser origin offset self-calibration based on sliding window point cloud overlapping

Abstract

The invention discloses a method for realizing laser origin offset self-calibration based on sliding window point cloud overlapping. The method comprises the steps of segmented scanning and data acquisition, sliding window data management, overlapping region joint optimization modeling, joint optimization solution and initial value estimation, physical offset time smooth filtering processing and point cloud coordinate correction and compensation updating. According to the method, a laser module is driven by a steering engine to perform segmented scanning, and an overlapping region with rich geometrical characteristics is automatically selected to obtain data; according to the method, an error model taking a physical offset vector d and a pose (R, T) as optimization variables is constructed, joint optimization is carried out by using a sparse Gaussian-Newton algorithm, and a stable compensation value is obtained through time smoothing filtering and is used for correcting point cloud coordinates; according to the method, an external target is not needed, the modeling precision and consistency of long-distance segmented scanning can be remarkably improved, and the method is suitable for real-time or offline processing.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of three-dimensional laser point cloud data processing, and particularly relates to a method for realizing laser origin offset self-calibration based on sliding window point cloud overlap. BACKGROUND

[0002] With the popularity of laser scanning equipment in the fields of engineering surveying and mapping and three-dimensional modeling, the application of low-cost, IMU-free rudder laser systems on mobile platforms such as mines, tunnels and robots is continuously expanding. One problem of the equipment is that the small assembly deviation between the laser emission origin and the rotation axis is continuously amplified in large-angle scanning, causing point cloud layer misplacement, strip bending and patch tearing, which is easy to produce significant drift in segmented splicing, and seriously reduces the three-dimensional reconstruction accuracy and geometric consistency. The existing methods usually rely on tactical IMU or bulky geometric targets for calibration, which is high in cost and difficult to lay out. One-time offline calibration is also difficult to resist dynamic errors such as temperature drift and mechanical loosening, and is easy to lose accuracy in long-term operation.

[0003] Therefore, there is an urgent need for a new method that does not require external equipment and can automatically estimate and continuously compensate the origin offset relying on overlapping point clouds in regular scanning or offline processing, to ensure that low-cost rudder laser systems can achieve high-precision, long-distance and stable consistent mapping. SUMMARY

[0004] To solve the above problems, the purpose of the present application is to provide a method for realizing laser origin offset self-calibration based on sliding window point cloud overlap. Under the conditions of no IMU and no target, the physical deviation caused by the mismatch between the laser emission origin and the rotation axis of the low-cost laser radar equipment assembly is constructed by coupling the residual error in the overlapping area of adjacent scanning segments, the shared physical offset vector d and the local pose are optimized simultaneously by sparse Gauss-Newton, and are smoothed to compensate the point cloud coordinates in real time and suppress the long-distance drift.

[0005] The purpose of the present application is achieved by the following steps: S100, segmented scanning and data acquisition: the target area is scanned by a rudder-driven laser ranging module. Before or during the scanning operation, key positions with rich geometric features (such as turning and cross-section changes) are manually pre-marked or real-time marked. When the scanning reaches the manually marked position, the scanning segment switching is preferentially executed to ensure that the most important overlapping area is collected with high quality. During the scanning process, the operating parameters are continuously monitored. When any over-limit condition is met, the current scanning segment is automatically ended and a new scanning segment is started. The original data including the rudder angle and distance information are collected by segmented scanning to form an original data stream containing multiple scanning data, and the boundary information of each scanning segment is recorded; Wherein, the rudder drive laser ranging module is a conventional laser scanning device in the art, meeting the adaptive acquisition requirement of the present application for the region with rich geometric features; the main control module can be optionally equipped with a tablet terminal or a handheld terminal, which can be extended to support manual marking of the high-feature region (triggering high-density sampling), remote configuration of scanning parameters, and auxiliary post-processing, and can be further miniaturized to realize a wearable design.

[0006] S200, sliding window data management: a sliding window containing the last N scanning segment data is maintained to record the boundary information of each scanning segment, the original data stream is divided into independent scanning segments, and each scanning segment is sequentially added to the window, that is, the oldest scanning segment is removed while the new scanning segment is added, ensuring that the data in the window is updated in real time, forming a locally continuous and overlapping data set; S300, joint optimization modeling of overlapping regions: in the sliding window, the laser origin physical offset vector d and the relative pose (R, T) between adjacent scanning segments are taken as joint optimization variables, an error cost function is constructed based on the geometric consistency of the overlapping region point cloud, and the matching error between the overlapping point clouds after pose transformation and offset compensation is minimized; S400, joint optimization solution and initial value estimation: the model of step S300 is solved by using a sparse Gauss-Newton method to obtain the optimal estimation value of the physical offset vector d and the relative pose (R, T), and the self-calibration of internal parameters and external motion is realized; S500, physical offset time smoothing filter processing: the physical offset vector d obtained in each optimization is input into a time smoothing filter, and a stable physical offset compensation value is output, local noise interference is suppressed, a change continuous and stable physical offset compensation value is output, and the stability and compensation robustness of the system during long-term operation are improved; S600, point cloud coordinate correction and compensation update: based on the current smoothed physical offset compensation value, the point cloud data collected in real time or recorded in history is corrected in coordinates, and the compensated three-dimensional point cloud is generated; the window continuously slides, and the point cloud coordinate correction of steps S200 to S600 is repeatedly executed, realizing online adaptive update and error continuous suppression of the physical offset.

[0007] Preferably, the over-limit condition in S100 step is: Geometric complexity over-limit: the geometric complexity index C of the point cloud in the current scanning segment exceeds the preset threshold θc; Scan angle over-limit: the cumulative rotation angle θ sum of the horizontal rudder exceeds the maximum range θ max ; Scan duration over-limit: the duration t seg of the current segment exceeds the maximum threshold ; Point cloud quantity exceeded: The number of point clouds N collected in the current segment pts The set upper limit N has been reached. max .

[0008] Preferably, the raw data includes pitch and horizontal servo angles, laser rangefinder values, and sampling timestamps, while the boundary information includes segment number, scan segment start and end time, angle range, and maximum complexity.

[0009] Preferably, after data acquisition in step S100 is completed, offline analysis is used as a supplementary segment to redetermine the end point of each scan segment. Specifically: The current scan segment will automatically end when any of the following conditions are met: an additional segment end marker will be inserted at the current sampling point, the accumulated amount will be reset to zero, and scanning will continue until the next manual marker is reached; the conditions are: The geometric complexity C exceeds the set threshold; Horizontal angle accumulation Exceeding the set maximum angle range (e.g., 45°); The sampling duration t_seg of the current segment exceeds the maximum threshold. (e.g., 15s); The number of point clouds collected, N_pts, reaches the maximum number of points, N_max (e.g., ); The calculated geometric complexity C reaches the threshold. ; The formula for calculating the geometric complexity C is as follows:

[0010] in, Geometric complexity; : Curvature variance within the scan segment; Normal vector entropy is the point cloud distribution; Density of interior corner points per unit voxel; These are the weighting coefficients for the curvature variance within the scan segment. The weighting coefficients of the normal vector entropy. This is the weighting coefficient for the density of corner points within a unit voxel.

[0011] Preferably, step S200, which divides the original data stream into independent scan segments, specifically involves reading the segment boundary information recorded in S100 and logically dividing the continuously sampled data into separate scan segments. Each segment contains Quadruple, where It's the pitch servo angle. It is the horizontal servo angle. It is a laser ranging value. It is a sampling timestamp, and it also parses the segment header metadata. The specific data includes the following:

[0012] Where k is the scan segment number; These represent the start and end timestamps of the data collection segment, respectively. These represent the scanning range of the pitch and horizontal angles, respectively. This represents the maximum value of the geometric complexity index calculated within this segment; S200 Step When New Scan Segment Adding a window means aligning it with the end of the window. Perform a three-step coarse registration to generate the initial relative pose of the segment. Let the offset be d = d_prev. Convert α, η, and ρ to Cartesian coordinates. Use FPFH features and RANSAC to extract point cloud features after voxel downsampling and perform pairing to initially estimate rotation and translation. Finally, perform ICP refinement and run one iteration of nearest-point registration within a set maximum distance threshold (e.g., 0.2m) to obtain the final coarse alignment result. ; Among them, the initial value of the offset d is estimated as follows: if the window is filled for the first time, it is set to the zero vector by default or estimated by solving the closed-form solution of the difference between the mean of the first and last segments; otherwise, it directly inherits the d* obtained from the previous optimization to speed up convergence and ensure a smooth transition of the initial value. The window structure caches the point cloud data, segment header metadata, relative pose (R,T) obtained from coarse registration, and the current initial value of d for each segment. The window quality is judged based on the overlap ratio and the local registration residual RMS, and dynamic adjustments are initially set. For example, when the overlap rate is insufficient or the residual is large, the window length N is increased to the upper limit N_max; when the overlap is sufficient and the optimization convergence is stable, N is reduced to the lower limit N_min to reduce the computational load; if the number of segments N in the window is greater than or equal to 2, the subsequent joint optimization modeling of the S300 overlapping area is triggered. After the optimization is completed, the window slides forward one position, and the system repeats the above process. This provides numerically observable and fast-converging input conditions for joint optimization.

[0013] Preferably, the joint optimization modeling of the overlapping region in step S300 specifically includes: S301, Optimization Goal: In the current sliding window The objective is to minimize the overlap error between any two adjacent scan segments. Specifically, the optimization targets are: A physical offset vector shared across the entire window This represents the static offset between the laser ranging origin and the servo rotation center; the relative pose between each pair of adjacent segments. , indicating the rigid body transformation of the (k+1)th point cloud segment relative to the kth segment; S302, Point-to-Pair Creation: For each pair of adjacent segments within the window... Extract the corresponding point pairs within their overlapping regions as the input data source for the subsequent residual function; S303. Construction of Joint Residual Function: After applying the physical offset d to the two segments, the residual is defined as:

[0014] in, This refers to the j-th point in the k-th segment; R represents the geometric error between corresponding points of the same structure in two frames of point clouds; k and T k For the relative rotation and translation between the kth pairs of segments, this residual characterizes the spatial inconsistency of the same structural point in two frames under the currently estimated laser offset and relative pose. S304. Residual Cost Function Construction and Optimization Structure: Based on the established set of point pairs, the overall optimization objective function is constructed as follows:

[0015] in, The information weight assigned to each point pair is calculated based on factors such as point density, normal difference, and overlapping information entropy within the region. : is a robust kernel function used to reduce the impact of outliers on the least squares objective.

[0016] Preferably, step S302, establishing point pairs, specifically includes: S3201, Coordinate Transformation: For each original sampling point (α, η, ρ), based on the currently estimated offset vector d, transform it into three-dimensional coordinates:

[0017] in, These are the rotation matrices corresponding to the pitch and horizontal angles, respectively. The transformation result is used to construct a Cartesian coordinate point cloud that has been compensated for geometric deviations. ; S3022. Filtering corresponding point pairs in overlapping areas: Filtering the transformed point cloud... Using kd-trees in Search for the nearest neighbor of a point in the given set of points, while setting three constraints. Only retain pairs of points that satisfy all of the following conditions: Set Euclidean distance constraint ,in This is the maximum matching distance threshold; Set the angle between the normal vectors It is used to remove points with inconsistent structures or obscured edges; To set corner overlap limits, point pairs must be located within the common field of view of two scan segments to ensure actual spatial overlap. For the selected valid point pairs, their coordinates, normals, and weights are written into the overlapping point pair set as direct inputs for subsequent joint residual calculation.

[0018] The S300 step constructs point-to-point geometric residuals within the overlapping region of each pair of adjacent scan segments, using the physical offset vector d and the relative pose (R,T) as joint optimization variables. The residuals are constructed based on the squared Euclidean distance and external point suppression is applied using the Huber kernel or other robust kernel functions. The residuals can be weighted according to the information entropy of the overlapping region. The point cloud can be first divided into voxel grid regions and grouped and weighted to form a Jacobian matrix with a block-band sparse structure, providing a robust and efficient error model for subsequent joint optimization solutions based on the sparse Gauss-Newton method.

[0019] Preferably, the joint optimization solution and initial value estimation in step S400 specifically include: S401. Sparse Linearization and Structure Preservation: Utilizing the structural characteristics of the residuals in the overlapping regions within the sliding window, the error function is linearized to order one at the beginning of each iteration to obtain the parameter increment model. Where J is the sparse Jacobian matrix corresponding to the residual, each row of this structure is associated with only three variable blocks: the shared offset vector d, the current segment pose, and the current position. and the next position The overall structure is a block-band sparse structure, which is suitable for directly constructing sparse normal equations, greatly reducing memory consumption and computation. This invention can run on resource-constrained embedded platforms and can also be adapted to larger window sizes to improve global convergence capabilities. S402. Construction and Solution of Joint Normal Equations: While maintaining the above structure, construct the normal equations of the current linearized system:

[0020] Wherein, J represents a blocky, banded, sparse Jacobian; This is the damping coefficient, used to improve convergence when the rank is not full or the initial error is large; The least squares increment for the joint variables; The sparse structure is inherited from J, and it has a highly optimized solution path; This invention uses the following method: by default, sparse Cholesky direct decomposition is adopted. If the number of window points increases or the system is running on a computing-limited platform, PCG iteration is switched and a block diagonal preconditioner is selected. The entire solution process only utilizes sparse structures and does not require the construction of dense Hessian. The computational complexity of the entire process is maintained at the O(n) level, which significantly reduces the sensitivity to initial values ​​and feature quality. It has higher convergence robustness and embedded adaptability than traditional ICP coarse registration. S403. Variable Update Strategy and Rotation Disturbance Handling: After completing the incremental solution, the optimization variables are updated synchronously: the translation and offset vectors are updated using direct addition. The rotation is updated using Lie algebraic perturbation to avoid violating Euler angle singularity or rotation matrix orthogonality. This is a key technical detail in the solution of three-dimensional rotation variables in this invention. The formula is as follows:

[0021] in, Indicates an antisymmetric matrix, Represents the exponential mapping on the SO(3) Lie group. This update method can preserve the integrability and accuracy of rotation optimization in the minimum dimension. S404. Convergence Criterion: The current window optimization is terminated when any of the following conditions are met; otherwise, the next iteration continues until convergence is achieved. The conditions are: Determination of residual decline rate: E prev E is the global error cost function value from the previous iteration. curr This represents the global error cost function value for the current iteration. The set threshold for the residual decay rate; Incremental amplitude threshold determination: ; Maximum number of iterations limit: L-max.

[0022] Preferably, the S500 step smoothing filter processing method uses an exponentially weighted moving average (EWMA) filter to recursively smooth the physical offset estimate obtained in each round of joint optimization, specifically updating it according to the following formula:

[0023] in, This is the smooth offset vector output for the current window; The original optimization results output by step S400; : Smoothing factor, controls response speed and jitter speed; t: Sliding window number; If there is a need to model the long-term drift trend, a first-order Kalman filter is used instead of an exponentially weighted moving average filter. The state covariance and dynamic error are introduced for modeling. This is suitable for scenarios where d changes slowly over time, such as temperature drift. However, additional modeling of process noise and observation noise is required. The smoothed offset estimate d will be used as the final reference value for point cloud coordinate correction in the next step and the initial value of the physical offset for the next window, so that the subsequent model can be uniformly repaired or reconstructed as a whole. Preferably, step S600 generates a compensated 3D point cloud. Specifically, for each sampling point (α, η, ρ), the optimal pose of the current window is applied. and smooth offset vector The corrected three-dimensional coordinates are calculated using the following formula:

[0024] in, For pitch and horizontal rotation; Add offset correction to the laser emission origin.

[0025] Preferably, the S600 step correction includes: Real-time mode: Immediately use the current data for each new sample point. Correct the pose and generate a visual point flow or incremental mapping. Offline mode: The cached historical raw data is corrected in batches segment by segment according to the sliding window order to achieve overall model repair and consistency adjustment.

[0026] Preferably, the key is the cyclical updating of the sliding window. Specifically, after correction, the window slides forward, removing the oldest segment and adding a new sampling segment, continuing the optimization and filtering process. This forms a closed-loop sequence of acquisition, segmentation, optimization, filtering, correction, and sliding, continuously suppressing errors and ensuring overall model consistency, adapting to long-term operation and multi-scenario deployment requirements. Each time the window slides, the system automatically estimates and updates the physical offset d and relative pose (R,T), preventing the accumulation of local errors during mapping and ensuring real-time performance and robustness in practical applications.

[0027] Compared with the prior art, the present invention has the following technical effects: 1. This invention does not require an expensive IMU or additional large calibration targets. Calibration is completed solely by the natural overlap of the scan segments, which greatly reduces hardware costs and the difficulty of on-site deployment. It is especially suitable for operation in narrow mine tunnels. 2. This invention uses sliding window local optimization, retaining only the latest N segment point clouds. The resulting sparse banded matrix structure significantly reduces computational and storage overhead by an order of magnitude compared to global adjustment, and can be run in real time in embedded systems. 3. This invention is the first to couple the laser origin offset d with the adjacent pose (R,T) in the same nonlinear framework, avoiding the spread of two-stage calibration error, reducing iterations, and simultaneously improving accuracy and convergence speed; 4. This invention uses a sparse Gauss-Newton algorithm and Lie group update, keeping the equations sparse. It can be solved quickly and linearly using Cholesky or PCG, allowing low-power CPUs to run it easily. 5. This invention effectively suppresses temperature drift and mechanical error accumulation through online smoothing compensation and closed-loop mapping process, maintains the geometric consistency of the model and the robustness of the system in multiple scenarios over a long period of time, and ensures robust and high-precision data output. Attached Figure Description

[0028] Figure 1 This is a schematic flowchart of the method of the present invention; Figure 2 This is a schematic diagram of sliding window data management; Figure 3 A schematic diagram of the point cloud for a sliding window; Figure 4 This is a schematic diagram illustrating the extraction of corresponding point pairs in the overlapping region and the construction of residuals. Figure 5 Flowchart for solving the sparse Gaussian-Newton joint optimization problem; Figure 6 This is a schematic diagram of the online EWMA smoothing filter compensation curve; Figure 7 This is a comparison chart showing the consistency of the 3D model before and after point cloud correction. Detailed Implementation

[0029] The present invention will be further described below with reference to the embodiments and accompanying drawings, but this does not limit the present invention in any way. Any changes or substitutions made based on the teachings of the present invention shall fall within the protection scope of the present invention. Example

[0030] The subject of this invention is an underground tunnel in a lead-silver mine in Yunnan Province. The mine is an underground mine that mines lead and silver. The production scale is 100,000 tons / year. The mining area is 0.3850 km² and is located in a certain county. The mine was established in 2006, but mining activities can be traced back to an earlier period. The current mining focus has shifted to the 2050m elevation.

[0031] As attached Figure 1 The method for self-calibrating laser origin offset based on sliding window point cloud overlap shown in this embodiment includes the following steps: S100. Segmented Scanning and Data Acquisition: This study conducted data acquisition on a 10M tunnel. The operator used a handheld laser scanning device to scan the tunnel in segments. The laser scanning device was integrated with commercially available standardized components: one MG996R servo motor was configured in both the horizontal and vertical directions to form a two-degree-of-freedom gimbal, equipped with an SDM50 laser transmitter as the ranging unit, and an ESP32 as the main control module (integrated Wi-Fi / Bluetooth, supporting data storage and transmission). Powered by a mobile power supply, it outputs raw data containing servo motor angle and distance information, meeting the adaptive acquisition requirements of this invention for areas with rich geometric features. Manual marking priority trigger: Before the scanning operation, key locations with rich geometric features are manually marked (marking turns, cross-sectional changes); when the scanning reaches the manually marked location, the scanning segment is switched first to ensure that the most important overlapping areas are acquired with high quality; during the scanning process, the operating parameters are continuously monitored, and if any limit condition is met, the current scanning segment is automatically ended and a new scanning segment is started; segmented scanning acquires raw data including servo angle and distance information, forming a raw data stream containing multiple scanning data segments, and the boundary information of each scanning segment is recorded; After the S100 step data acquisition is completed, offline analysis is used as a supplementary segment to redetermine the end point of each scan segment. Specifically: Multi-condition parallel automatic triggering: When any of the following conditions are met, the current scan segment will automatically end, an additional segment end marker will be inserted at the current sampling point, and the accumulated amount will be reset to zero before continuing the scan until the next manual marker is reached; the conditions are: The geometric complexity C exceeds the set threshold; Horizontal angle accumulation Exceeding the set maximum angle range Take 45°; The sampling duration t_seg of the current segment exceeds the maximum threshold. Take 15 seconds; When the number of point clouds collected, N_pts, reaches the maximum number of points, N_max, take... ; The calculated geometric complexity C reaches the threshold. ; The formula for calculating the geometric complexity C is as follows:

[0032] in, Geometric complexity; : Curvature variance within the scan segment; Normal vector entropy is the point cloud distribution; Density of interior corner points per unit voxel; These are the weighting coefficients for the curvature variance within the scan segment. The weighting coefficients of the normal vector entropy. The weighting coefficient for the density of corner points within a unit voxel; The two methods above (manual marking priority trigger and multi-condition parallel automatic trigger) can work together to trigger the scan segment switching. When the scan reaches the manually marked position, the system will prioritize the switching of the scan segment to ensure that the most important overlapping area is acquired with high quality. The multi-condition parallel automatic trigger adaptive mechanism serves as a supplement to the manual marking method.

[0033] The raw data includes pitch and horizontal servo angles, laser rangefinder values, and sampling timestamps. Boundary information includes segment number, start and end times of the scan segment, angle range, and maximum complexity. The condition for exceeding limits is: Geometric complexity exceeded: The geometric complexity index C of the point cloud in the current scan segment exceeds the preset threshold θc; Scan angle exceeded limit: The cumulative rotation angle θ of the horizontal servo motor sum Exceeding the maximum range θ max ; Scan duration exceeded limit: The continuous acquisition time t of the current segment seg Exceeding the maximum threshold ; Point cloud quantity exceeded: The number of point clouds N collected in the current segment pts The set upper limit N has been reached. max ; S200, Sliding Window Data Management: Maintain a sliding window containing the data of the most recent N scan segments. Record the boundary information of each scan segment to divide the original data stream into independent scan segments. Add each scan segment to the window in sequence. That is, remove the oldest scan segment while adding a new scan segment to ensure that the data in the window is updated in real time and form a locally continuous data set with overlapping relationships. Step S200 divides the raw data stream into independent scan segments, specifically as shown in the attached diagram. Figure 2 As shown, the point cloud is segmented in the sliding window. The window reads the original sampling data and segment boundary marker table recorded by S100, and logically divides the continuous sampling data into separate scan segments. Each segment contains Quadruple, where It's the pitch servo angle. It is the horizontal servo angle. It is a laser ranging value. It is a sampling timestamp, and it also parses the segment header metadata. The specific data includes the following:

[0034] Where k is the scan segment number; These represent the start and end timestamps of the data collection segment, respectively. These represent the scanning range of the pitch and horizontal angles, respectively. This represents the maximum value of the geometric complexity index calculated within this segment; S200 Step When New Scan Segment Adding a window means aligning it with the end of the window. Perform a three-step coarse registration to generate the initial relative pose of the segment. Let the offset be d = d_prev. Convert α, η, and ρ to Cartesian coordinates. Use FPFH features and RANSAC to extract point cloud features after voxel downsampling and perform pairing to initially estimate rotation and translation. Finally, perform ICP refinement and run one iteration of nearest-point registration within a set maximum distance threshold (e.g., 0; 2m) to obtain the final coarse alignment result. ; Among them, the initial value of the offset d is estimated as follows: if the window is filled for the first time, it is set to the zero vector by default or estimated by solving the closed-form solution of the difference between the mean of the first and last segments; otherwise, it directly inherits the d* obtained from the previous optimization to speed up convergence and ensure a smooth transition of the initial value. The window structure caches the point cloud data, segment header metadata, relative pose (R,T) obtained from coarse registration, and the current initial value of d for each segment. The window quality is judged based on the overlap ratio and the local registration residual RMS, and dynamic adjustment is initially set. This includes increasing the window length N to the upper limit N_max when the overlap rate is insufficient or the residual is large; and reducing N to the lower limit N_min when the overlap is sufficient and the optimization convergence is stable to reduce the computational load. If the number of segments N in the window is greater than or equal to 2, the subsequent joint optimization modeling of the S300 overlapping area is triggered. After the optimization is completed, the window slides forward one position, and the system repeats the above process. This provides numerically observable and fast-converging input conditions for joint optimization. S300, Joint Optimization Modeling of Overlapping Regions: See attached Figure 3 As shown, within the sliding window, the physical offset vector d of the laser origin and the relative pose (R,T) between adjacent scan segments are used as joint optimization variables. An error cost function is constructed based on the geometric consistency of the point cloud in the overlapping region to obtain the matching error between the overlapping point clouds after pose transformation and offset compensation, minimizing the error. Step S300 specifically includes: S301, Optimization Goal: In the current sliding window The objective is to minimize the overlap error between any two adjacent scan segments. Specifically, the optimization targets are: A physical offset vector shared across the entire window This represents the static offset between the laser ranging origin and the servo rotation center; the relative pose between each pair of adjacent segments. , indicating the rigid body transformation of the (k+1)th point cloud segment relative to the kth segment; S302, Point-to-Pair Creation: For each pair of adjacent segments within the window... Extracting corresponding point pairs within their overlapping regions serves as the input data source for the subsequent residual function; step S302, point pair establishment, specifically includes: S3201, Coordinate Transformation: For each original sampling point (α, η, ρ), based on the currently estimated offset vector d, transform it into three-dimensional coordinates:

[0035] in, These are the rotation matrices corresponding to the pitch and horizontal angles, respectively. The transformation result is used to construct a Cartesian coordinate point cloud that has been compensated for geometric deviations. ; S3022. Filtering corresponding point pairs in overlapping areas: Filtering the transformed point cloud... Using kd-trees in Search for the nearest neighbor of a point in the given set of points, while setting three constraints. Only retain pairs of points that satisfy all of the following conditions: Set Euclidean distance constraint ,in This is the maximum matching distance threshold; Set the angle between the normal vectors It is used to remove points with inconsistent structures or obscured edges; To set corner overlap limits, point pairs must be located within the common field of view of two scan segments to ensure actual spatial overlap. For the selected valid point pairs, their coordinates, normals, and weights are written into the overlapping point pair set as direct inputs for subsequent joint residual calculations; S303. Construction of Joint Residual Function: After applying the physical offset d to the two segments, the residual is defined as:

[0036] Where is the j-th point in the k-th segment; represents the geometric error between corresponding points of the same structure in two frame point clouds; R k and T k For the relative rotation and translation between the kth pairs of segments, this residual characterizes the spatial inconsistency of the same structural point in two frames under the currently estimated laser offset and relative pose. S304. Residual Cost Function Construction and Optimization Structure: Based on the established set of point pairs, the overall optimization objective function is constructed as follows:

[0037] in, The information weight assigned to each point pair is calculated based on factors such as point density, normal difference, and overlapping information entropy within the region. This is a robust kernel function used to reduce the impact of outliers on the least squares objective. In this embodiment, the Huber kernel is used by default, and the threshold is set... Set to 0.05m; however, the present invention is not limited to this specific form and parameters; S400, Joint Optimization Solution and Initial Value Estimation: As attached Figure 4As shown, the sparse Gaussian-Newton method is used to solve the model in step S300 to obtain the optimal estimates of the physical offset vector d and the relative pose (R,T), thus achieving self-calibration of internal parameters and external motion quantities; step S400 specifically includes: S401. Sparse Linearization and Structure Preservation: Utilizing the structural characteristics of the residuals in the overlapping regions within the sliding window, the error function is linearized to order one at the beginning of each iteration to obtain the parameter increment model. Where J is the sparse Jacobian matrix corresponding to the residual, each row of this structure is associated with only three variable blocks: the shared offset vector d, the current segment pose, and the current position. and the next position The overall structure is a block-band sparse structure, which is suitable for directly constructing sparse normal equations, greatly reducing memory consumption and computation. This invention can run on resource-constrained embedded platforms and can also be adapted to larger window sizes to improve global convergence capabilities. S402. Construction and Solution of Joint Normal Equations: While maintaining the above structure, construct the normal equations of the current linearized system:

[0038] Wherein, J represents a blocky, banded, sparse Jacobian; This is the damping coefficient, used to improve convergence when the rank is not full or the initial error is large; The least squares increment for the joint variables; The sparse structure is inherited from J, and it has a highly optimized solution path; S403. Variable Update Strategy and Rotation Disturbance Handling: After completing the incremental solution, the optimization variables are updated synchronously: the translation and offset vectors are updated using direct addition. The rotation is updated using Lie algebraic perturbation to avoid violating Euler angle singularity or rotation matrix orthogonality. This is a key technical detail in the solution of three-dimensional rotation variables in this invention. The formula is as follows:

[0039] in, Indicates an antisymmetric matrix, Represents the exponential mapping on the SO(3) Lie group. This update method can preserve the integrability and accuracy of rotation optimization in the minimum dimension. S404. Convergence Criterion: The current window optimization is terminated when any of the following conditions are met; otherwise, the next iteration continues until convergence is achieved. The conditions are: Determination of residual decline rate: E prev E is the global error cost function value from the previous iteration. curr This represents the global error cost function value for the current iteration. The set threshold for the residual decay rate; Incremental amplitude threshold determination: ; Maximum number of iterations limit: L-max; S500, Physical Offset Time Smoothing Filtering: The physical offset vector d obtained from each round of optimization is input into a time smoothing filter, which outputs a stable physical offset compensation value, suppresses local noise interference, and outputs a continuously changing and stable physical offset compensation value, thereby improving the long-term stability and compensation robustness of the system; as shown in the appendix. Figure 5 As shown, the smoothing filter method uses an exponentially weighted moving average (EWMA) filter to recursively smooth the physical offset estimate obtained in each round of joint optimization, specifically updated according to the following formula:

[0040] in, This is the smooth offset vector output for the current window; The original optimization results output by step S400; : Smoothing factor, controls response speed and jitter speed; t: Sliding window number; If there is a need to model the long-term drift trend, a first-order Kalman filter is used instead of an exponentially weighted moving average filter. The state covariance and dynamic error are introduced for modeling. This is suitable for scenarios where d changes slowly over time, such as temperature drift. However, additional modeling of process noise and observation noise is required. The smoothed offset estimate d will be used as the final reference value for point cloud coordinate correction in the next step and the initial value of the physical offset for the next window, so that the subsequent model can be uniformly repaired or reconstructed as a whole. S600, Point Cloud Coordinate Correction and Compensation Update: Based on the current smoothed physical offset compensation value, coordinate correction is performed on the real-time acquired or historical point cloud data to generate a compensated 3D point cloud; the window continues to slide, repeating steps S200 to S600 for point cloud coordinate correction, achieving online adaptive update of physical offset and continuous error suppression; specifically, generating the compensated 3D point cloud involves: for each sampling point (α, η, ρ), applying the current window's optimal pose. and smooth offset vector The corrected three-dimensional coordinates are calculated using the following formula:

[0041] in, For pitch and horizontal rotation; Add offset correction to the laser emission origin; S600 step calibration includes: Real-time mode: Immediately use the current data for each new sample point. Correct the pose and generate a visual point flow or incremental mapping. Offline mode: The cached historical raw data is corrected in batches segment by segment according to the sliding window order to achieve overall model repair and consistency adjustment; The sliding window's cyclical update is as follows: After correction, the window slides forward, removing the oldest segment and adding a new sampling segment, continuing the optimization and filtering process. This forms a closed-loop sequence of acquisition, segmentation, optimization, filtering, correction, and sliding, continuously suppressing errors and ensuring overall model consistency, adapting to long-term operation and multi-scenario deployment requirements. Each time the window slides, the system automatically estimates and updates the physical offset d and relative pose (R,T), preventing the accumulation of local errors during mapping and ensuring real-time performance and robustness in practical applications. As attached Figure 6 As shown, the changes in the 3D model before and after the final correction of the point cloud in the tunnel entrance area are as follows: Before correction on the left, the point cloud has obvious strip curvature in the Y-axis (the direction of tunnel extension) due to the lack of compensation for the offset between the laser origin and the rotation axis. There is also tearing of the facets at the corners, and significant misalignment of the same structure in the overlapping area of ​​adjacent scanning segments (the maximum deviation in the height direction of the Z-axis reaches 5-8cm). After correction on the right, the strip curvature is completely eliminated through sliding window joint optimization and EWMA smoothing. The facets at the corners are seamlessly stitched together, and the geometric consistency of the overlapping area is significantly improved. This intuitively demonstrates the effective compensation of the laser origin offset and the improvement of the accuracy of the 3D model by this invention.

Claims

1. A method for self-calibrating laser origin offset based on sliding window point cloud overlap, characterized in that... Includes the following steps: S100, Segmented Scanning and Data Acquisition: The laser ranging module driven by a servo motor performs segmented scanning of the target area. Before or during the scanning operation, key locations with rich geometric features are marked manually or in real time. When the scanning reaches the manually marked location, the scanning segment is switched first. During the scanning process, the operating parameters are continuously monitored. If any limit condition is met, the current scanning segment is automatically terminated and a new scanning segment is started. The segmented scanning acquires raw data including servo motor angle and distance information, forming a raw data stream containing multiple scanning data segments, and records the boundary information of each scanning segment. S200, Sliding Window Data Management: Maintain a sliding window containing the data of the most recent N scan segments. Record the boundary information of each scan segment to divide the original data stream into independent scan segments. Add each scan segment to the window in sequence. That is, remove the oldest scan segment while adding a new scan segment to ensure that the data in the window is updated in real time and form a locally continuous data set with overlapping relationships. S300, Joint optimization modeling of overlapping regions: Within the sliding window, the physical offset vector d of the laser origin and the relative pose (R,T) between each adjacent scanning segment are used as joint optimization variables. An error cost function is constructed based on the geometric consistency of the point cloud in the overlapping region to obtain the minimum matching error between the overlapping point cloud after pose transformation and offset compensation. S400, Joint Optimization Solution and Initial Value Estimation: The model in step S300 is solved using the sparse Gauss-Newton method to obtain the optimal estimates of the physical offset vector d and the relative pose (R,T), thereby achieving self-calibration of internal parameters and external motion quantities. S500 Physical Offset Time Smoothing Filtering: Input the physical offset vector d obtained from each round of optimization into the time smoothing filter, output a stable physical offset compensation value, suppress local noise interference, and output a continuously changing and stable physical offset compensation value. S600, Point Cloud Coordinate Correction and Compensation Update: Based on the current smoothed physical offset compensation value, coordinate correction is performed on the real-time acquired or historical point cloud data to generate a compensated 3D point cloud; the window continues to slide, and the point cloud coordinate correction steps from S200 to S600 are repeated to enable online adaptive updating of physical offset and continuous suppression of error.

2. The method for self-calibration of laser origin offset based on sliding window point cloud overlap according to claim 1, characterized in that... The condition for exceeding the limit in step S100 is: Geometric complexity exceeded: The geometric complexity index C of the point cloud in the current scan segment exceeds the preset threshold θc; Scan angle exceeded limit: The cumulative rotation angle θ of the horizontal servo motor sum Exceeding the maximum range θ max ; Scan duration exceeded limit: The continuous acquisition time t of the current segment seg Exceeding the maximum threshold ; Point cloud quantity exceeded: The number of point clouds N collected in the current segment pts The set upper limit N has been reached. max .

3. The method for self-calibrating laser origin offset based on sliding window point cloud overlap according to claim 1, characterized in that... The raw data includes pitch and horizontal servo angles, laser rangefinder values, and sampling timestamps. Boundary information includes segment number, scan segment start and end time, angle range, and maximum complexity.

4. The method for self-calibrating laser origin offset based on sliding window point cloud overlap according to claim 1, characterized in that... After the S100 step data acquisition is completed, offline analysis is used as a supplementary segment to redetermine the end point of each scan segment. Specifically: The current scan segment will automatically end when any of the following conditions are met: an additional segment end marker will be inserted at the current sampling point, the accumulated amount will be reset to zero, and scanning will continue until the next manual marker is reached; the conditions are: The geometric complexity C exceeds the set threshold; Horizontal angle accumulation Exceeding the set maximum angle range ; The sampling duration t_seg of the current segment exceeds the maximum threshold. ; The number of point clouds collected, N_pts, reached the maximum number of points, N_max. The calculated geometric complexity C reaches the threshold. ; The formula for calculating the geometric complexity C is as follows: ； in, Geometric complexity; : Curvature variance within the scan segment; Normal vector entropy is the point cloud distribution; Density of interior corner points per unit voxel; These are the weighting coefficients for the curvature variance within the scan segment. The weighting coefficients of the normal vector entropy. This is the weighting coefficient for the density of corner points within a unit voxel.

5. The method for self-calibrating laser origin offset based on sliding window point cloud overlap according to claim 1, characterized in that... Step S200 divides the raw data stream into independent scan segments by: reading the segment boundary information recorded in S100 and logically dividing the continuously sampled data into separate scan segments. Each segment contains Quadruple, where It's the pitch servo angle. It is the horizontal servo angle. It is a laser ranging value. It is a sampling timestamp, and it also parses the segment header metadata. The specific data includes the following: ； Where k is the scan segment number; These represent the start and end timestamps of the data collection segment, respectively. These represent the scanning range of the pitch and horizontal angles, respectively. This represents the maximum value of the geometric complexity index calculated within this segment; S200 Step When New Scan Segment Adding a window means aligning it with the end of the window. Perform a three-step coarse registration to generate the initial relative pose of the segment. Let the offset be d = d_prev for now, and convert α, η, and ρ to Cartesian coordinates. Utilize FPFH features and RANSAC to extract point cloud features after voxel downsampling and perform pairing to initially estimate rotation and translation. Finally, perform ICP refinement and run one iteration of nearest-point registration within a set maximum distance threshold to obtain the final coarse alignment result. ; Among them, the initial value of the offset d is estimated as follows: if the window is filled for the first time, it is set to the zero vector by default or estimated by solving the closed-form solution of the difference between the mean of the first and last segments; otherwise, it directly inherits the d* obtained from the previous optimization to speed up convergence and ensure a smooth transition of the initial value. The window structure caches the point cloud data, segment header metadata, relative pose (R,T) obtained from coarse registration, and the current initial value of d for each segment. The window quality is judged based on the overlap ratio and the local registration residual RMS, and dynamic adjustments are initially set. For example, when the overlap rate is insufficient or the residual is large, the window length N is increased to the upper limit N_max; when the overlap is sufficient and the optimization convergence is stable, N is reduced to the lower limit N_min to reduce the computational load; if the number of segments N in the window is greater than or equal to 2, the subsequent joint optimization modeling of the S300 overlapping area is triggered. After the optimization is completed, the window slides forward one position, and the system repeats the above process to provide input conditions for joint optimization.

6. The method for self-calibrating laser origin offset based on sliding window point cloud overlap according to claim 1, characterized in that... The joint optimization modeling of overlapping regions in the S300 steps specifically includes: S301, Optimization Goal: In the current sliding window The objective is to minimize the overlap error between any two adjacent scan segments. Specifically, the optimization targets are: A physical offset vector shared across the entire window This represents the static offset between the laser ranging origin and the servo rotation center; the relative pose between each pair of adjacent segments. , indicating the rigid body transformation of the (k+1)th point cloud segment relative to the kth segment; S302, Point-to-Pair Creation: For each pair of adjacent segments within the window... Extract the corresponding point pairs within their overlapping regions as the input data source for the subsequent residual function; S303. Construction of Joint Residual Function: After applying the physical offset d to the two segments, the residual is defined as: ； in, This refers to the j-th point in the k-th segment; R represents the geometric error between corresponding points of the same structure in two frames of point clouds; k and T k This represents the relative rotation and translation between the kth pairs of segments; S304. Residual Cost Function Construction and Optimization Structure: Based on the established set of point pairs, the overall optimization objective function is constructed as follows: ； in, The information weight assigned to each point pair is calculated based on factors such as point density, normal difference, and overlapping information entropy within the region; : is a robust kernel function used to reduce the impact of outliers on the least squares objective.

7. The method for self-calibrating laser origin offset based on sliding window point cloud overlap according to claim 6, characterized in that... The S302 step of establishing point pairs specifically includes: S3201, Coordinate Transformation: For each original sampling point (α, η, ρ), based on the currently estimated offset vector d, transform it into three-dimensional coordinates: ； in, These are the rotation matrices corresponding to the pitch and horizontal angles, respectively. The transformation result is used to construct a Cartesian coordinate point cloud that has been compensated for geometric deviations. ; S3022. Filtering corresponding point pairs in overlapping areas: Filtering the transformed point cloud... Using kd-trees in Search for the nearest neighbor of a point in the given set of points, while setting three constraints. Only retain pairs of points that satisfy all of the following conditions: Set Euclidean distance constraint ,in This is the maximum matching distance threshold; Set the angle between the normal vectors It is used to remove points with inconsistent structures or obscured edges; To set corner overlap limits, point pairs must be located within the common field of view of two scan segments to ensure actual spatial overlap. For the selected valid point pairs, their coordinates, normals, and weights are written into the overlapping point pair set as direct inputs for subsequent joint residual calculation.

8. The method for self-calibrating laser origin offset based on sliding window point cloud overlap according to claim 1, characterized in that... The S400 steps of joint optimization solution and initial value estimation specifically include: S401. Sparse Linearization and Structure Preservation: Utilizing the structural characteristics of the residuals in the overlapping regions within the sliding window, the error function is linearized to order one at the beginning of each iteration to obtain the parameter increment model. Where J is the sparse Jacobian matrix corresponding to the residual, each row of this structure is associated with only three variable blocks: the shared offset vector d, the current segment pose, and the current position. and the next position The overall structure is a blocky, sparse band structure; S402. Construction and Solution of Joint Normal Equations: While maintaining the above structure, construct the normal equations of the current linearized system: ； Wherein, J represents a blocky, banded, sparse Jacobian; The damping coefficient; The least squares increment for the joint variables; The sparse structure is inherited from J; S403. Variable Update Strategy and Rotation Disturbance Handling: After completing the incremental solution, the optimization variables are updated synchronously: the translation and offset vectors are updated using direct addition. The rotation is updated using Lie algebraic perturbation, as shown in the following formula: ； in, Indicates an antisymmetric matrix, Represents an exponential mapping on the SO(3) Lie group; S404. Convergence Determination: The current window optimization is terminated when any of the following conditions are met; otherwise, the next iteration continues until convergence is achieved. The conditions are: Determination of residual decline rate: E prev E is the global error cost function value from the previous iteration. curr This represents the global error cost function value for the current iteration. The set threshold for the residual decay rate; Incremental amplitude threshold determination: ; Maximum number of iterations limit: L-max.

9. The method for self-calibration of laser origin offset based on sliding window point cloud overlap according to claim 1, characterized in that... The S500 step smoothing filter method uses an exponentially weighted moving average filter to recursively smooth the physical offset estimate obtained from each round of joint optimization, specifically updating it according to the following formula: ； in, This is the smooth offset vector output for the current window; The original optimization results output by step S400; : Smoothing factor, controls response speed and jitter speed; t: Sliding window number; If there is a need to model the long-term drift trend, a first-order Kalman filter is used instead of the exponentially weighted moving average filter, and state covariance and dynamic error are introduced for modeling. The smoothed offset estimate d will be used as the final reference value for point cloud coordinate correction in the next step and the initial value of physical offset for the next window.

10. The method for self-calibrating laser origin offset based on sliding window point cloud overlap according to claim 1, characterized in that... The S600 step generates a compensated 3D point cloud. Specifically, for each sampling point (α, η, ρ), the optimal pose of the current window is applied. and smooth offset vector The corrected three-dimensional coordinates are calculated using the following formula: ； in, For pitch and horizontal rotation; Add offset correction to the laser emission origin.

11. The method for self-calibrating laser origin offset based on sliding window point cloud overlap according to claim 10, characterized in that... S600 step calibration includes: Real-time mode: Immediately use the current data for each new sample point. Correct the pose and generate a visual point flow or incremental mapping. Offline mode: Corrects cached historical raw data segment by segment in a sliding window order.

12. The method for self-calibration of laser origin offset based on sliding window point cloud overlap as described in claim 11, characterized in that... The sliding window is updated cyclically as follows: after the correction is completed, the window slides forward, removes the oldest segment and adds a new sampled segment, and continues to perform the optimization and filtering process. This forms a closed loop of acquisition, segmentation, optimization, filtering, correction and sliding, which continuously suppresses errors and ensures the consistency of the overall model. Each time the window slides, the physical offset d and the relative pose (R,T) are automatically estimated and updated.

Images