Spraying robot laser radar inertia tight coupling positioning method based on imu vibration spectrum sensing

Abstract

The application discloses a spraying robot laser radar inertia tight coupling positioning method based on IMU vibration spectrum sensing, comprising the following steps: 1) collecting IMU original data and preprocessing, and constructing a multi-dimensional standardized ground vibration fingerprint; 2) receiving the output multi-dimensional standardized ground vibration fingerprint, and realizing integrated decision of ground state sensing and positioning risk assessment; 3) according to the output decision triple, dynamically matching a pre-established multi-dimensional mapping library of “ground type-vibration interference level-treatment strategy”; 4) fusing the output point cloud without vibration noise and distortion and IMU pre-integration data, realizing adaptive tight coupling state optimization, and outputting pose estimation residual error; 5) based on the output pose estimation residual error, evaluating the mismatch degree of the current positioning system parameters and the environment, and realizing full-link closed-loop adaptive optimization. The application realizes integrated decision of ground type identification and positioning risk, and solves the problem that ground sensing and positioning decision are disconnected.

Description

Technical Field

[0001] This invention relates to the field of intelligent spraying robots and navigation and positioning technology, and in particular to a spraying robot lidar inertial tight coupling positioning method based on IMU vibration spectrum sensing. Background Technology

[0002] Uneven surfaces can easily generate severe vibrations, leading to laser point cloud distortion and positioning drift. Currently, sensors widely used for environmental perception include cameras, millimeter-wave radar, lidar, and inertial measurement units (IMUs). Among these, the tightly coupled lidar and IMU positioning system has become the mainstream technology in the field of mobile vehicle positioning due to its ability to provide high-frequency, high-precision pose estimation, especially suitable for the autonomous operation and positioning needs of intelligent spraying robots in complex environments. A typical tightly coupled lidar and IMU positioning system usually includes a lidar module, an IMU module, a point cloud preprocessing module, and a state estimation and optimization module; its working principle is as follows: first, laser point cloud and IMU motion data are collected, and then the two types of data are fused to calculate the robot's position and attitude, achieving autonomous positioning.

[0003] Chinese patent CN113391300A proposes a real-time motion compensation method for 3D point clouds based on an IMU (Integrated Device Array) lidar system. The main idea is to divide the point cloud data into data blocks according to the IMU output time sequence, calculate the three-axis rotation transformation matrix of each data point relative to the frame end for rotation compensation, and then estimate the inter-frame motion of the point cloud for translation compensation. While this patent technology improves the real-time performance and robustness of point cloud motion compensation to some extent, it performs poorly in estimating three-axis position changes. Furthermore, its point cloud motion compensation process uses a fixed low-order interpolation strategy and static compensation parameters, failing to consider the amplification of IMU pre-integration errors under high vibration scenarios. It lacks a mechanism to dynamically adjust the interpolation order and compensation accuracy based on the intensity of vibration. Consequently, in high-speed operation or high-vibration scenarios of intelligent spraying robots, the fixed interpolation model cannot match high-frequency pose changes, leading to inaccurate point cloud correction or loss of local geometric features, making it difficult to eliminate point cloud errors caused by ground vibration at the source.

[0004] Chinese patent CN116878492A proposes a multi-sensor fusion positioning method based on lidar and IMU. Its main idea is to use an Iterative Error State Kalman Filter (IESKF) as a tightly coupled positioning system, utilizing an IMU pre-integration model for state prediction, and combining this with a lidar registration error model for iterative optimization to correct pose. While this patented technology improves positioning accuracy and reliability to some extent in conventional scenarios, it employs a single, fixed IESKF optimization framework and observation model. Once the error state prediction and update equations are set, they remain unchanged, lacking a mechanism for dynamic switching between different filter frameworks when facing complex field terrain. Furthermore, it fails to link the vibration risk sensed at the front end with the weight adjustment of the back-end filter. This makes the fixed framework prone to filter divergence and positioning drift when the spraying robot encounters severe vibrations that cause geometric feature degradation.

[0005] Furthermore, the existing system architecture has two limitations: First, the ground type recognition and positioning processing strategies are independent. The recognition module only outputs the ground type without simultaneously assessing positioning risks, preventing the robot from adjusting its positioning strategy in advance when switching work areas. Second, the system adopts an open-loop architecture, with data transmitted unidirectionally between each stage. It lacks a closed-loop mechanism to feed back the pose estimation residuals to the front end for dynamic parameter adjustment, making it difficult to adapt to dynamically changing ground environments. These problems prevent the existing system from forming a complete closed loop from perception to decision-making to feedback optimization when facing field vibrations, making it difficult to guarantee long-term operational accuracy. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention provides a tightly coupled inertial positioning method for spraying robots based on IMU vibration spectrum sensing using lidar. It employs a multi-dimensional standardized ground vibration fingerprint combined with a CNN-Transformer network model to achieve integrated decision-making on ground type identification and positioning risks, thus resolving the disconnect between ground sensing and positioning decision-making. Based on V... level The interpolation order and filter coefficients are dynamically adjusted to solve the problem of inaccurate point cloud correction in high vibration scenarios; a dual-filter architecture of MSCKF and IESKF is constructed to dynamically switch the filtering strategy and adjust the observation weight to solve the problem of complex ground positioning drift; a pose estimation residual closed-loop feedback is introduced to dynamically adjust the front-end parameters throughout the entire link, so as to achieve system autonomous adaptation and continuous optimization.

[0007] The objective of this invention is achieved as follows: a tightly coupled inertial positioning method for a spraying robot based on IMU vibration spectrum sensing using lidar, comprising the following steps:

[0008] Step 1) Collect raw IMU data and preprocess it. Extract the frequency domain power spectral density (PSD) features through Fast Fourier Transform (FFT) and construct a multidimensional standardized ground vibration fingerprint by combining the time domain quantization features.

[0009] Step 2) Receive the multidimensional normalized ground vibration fingerprint output from Step 1). Utilize a pre-trained lightweight convolutional neural network (CNN) and a Transformer network model for capturing long-sequence signal dependencies to achieve integrated decision-making for ground state perception and positioning risk assessment. The output includes ground type information. Vibration interference level and laser positioning degradation risk coefficient Structured decision triples;

[0010] Step 3) Based on the decision triplet output in Step 2), dynamically match the pre-established multi-dimensional mapping library of "ground type-vibration interference level-processing strategy", extract the interpolation order and filtering coefficients corresponding to the current scene, and complete the adaptive distortion correction and vibration compensation of the point cloud accordingly, and output the point cloud with vibration noise and distortion removed.

[0011] Step 4) Fuse the vibration-noise- and distortion-free point cloud and IMU pre-integrated data output from Step 3), and based on the laser positioning degradation risk coefficient in the decision triplet output from Step 2). Vibration disturbance level The system dynamically switches between the multi-state constrained Kalman filter (MSCKF) and the iterative error state Kalman filter (IESKF) frameworks, and adjusts the observation weights to avoid positioning drift caused by laser observation under severe vibration, thereby achieving adaptive tightly coupled state optimization and outputting pose estimation residuals.

[0012] Step 5) Based on the pose estimation residuals output in Step 4), assess the degree of mismatch between the current positioning system parameters and the environment, and provide feedback to adjust the front-end parameters in Steps 1) and 3) to correct the accumulated error and achieve end-to-end closed-loop adaptive optimization.

[0013] Furthermore, step 1) includes:

[0014] Step 1-1) Data Acquisition and Preprocessing: Synchronously acquire raw IMU data, and after zero bias removal, filtering, and pre-integration, extract signal segments through a time sliding window;

[0015] Steps 1-2) Frequency domain feature extraction: In order to capture the local frequency response differences of the ground, the vibration signal within the window is subjected to Fast Fourier Transform (FFT) to extract the frequency domain power spectral density (PSD) features, which are used as the frequency domain input for ground type identification;

[0016] Steps 1-3) Time-domain feature quantization: Calculate the time-domain quantization features of the signal within the window, including the root mean square of the triaxial composite acceleration and the root mean square of the triaxial composite angular velocity. The calculation formula is as follows:

[0017]

[0018]

[0019] In the formula, This represents the root mean square of the triaxial composite acceleration. This represents the root mean square of the combined angular velocity of the three axes. Indicates the number of data points within the window. These represent the triaxial acceleration measurements at time k. These represent the triaxial angular velocity measurements at time k; the root mean square value is used to quantify the ground vibration intensity, with larger values ​​indicating more severe vibration.

[0020] Steps 1-4) Generation of Multidimensional Normalized Ground Vibration Fingerprint: The frequency domain power spectral density (PSD) features are normalized using the L2 norm to preserve the relative strength relationships at each frequency point; and Sliding window Min-Max normalization is performed to map the frequency domain features to the [0,1] interval; the normalized frequency domain features and time domain quantization features together constitute a multidimensional standardized ground vibration fingerprint.

[0021] Furthermore, step 2) includes:

[0022] Step 2-1) Multidimensional Normalized Ground Vibration Fingerprint Data Access: Receive the multidimensional normalized ground vibration fingerprint generated in Step 1), shape the normalized frequency domain features into a spectral tensor, and use it as input to the CNN branch; simultaneously, maintain a fixed-length T-frame temporal quantization feature queue, and store the temporal quantization features of the most recent T frames (…). , This is constructed as a time series and used as input for the Transformer branch;

[0023] Step 2-2) Hybrid Model Inference: Input the spectral features and temporal quantization features into a pre-trained lightweight CNN-Transformer hybrid model. The specific structure of the hybrid model is as follows:

[0024] The Convolutional Neural Network (CNN) branch consists of three one-dimensional convolutional layers with kernel sizes of 5, 3, and 3, and channel numbers of 32, 64, and 128, respectively. Each layer is followed by a max-pooling layer. This branch receives a spectral tensor and is used to extract local frequency response patterns that cannot be represented by single temporal statistics.

[0025] Transformer branch: Consists of two stacked Transformer encoders, each containing a multi-head self-attention mechanism (number of heads = 4, hidden dimension = 64) and a feedforward network; This branch receives a queue of temporal quantized features to capture long-range cumulative trends and dependencies that are easily ignored by single frequency domain features.

[0026] Feature fusion and multi-task output head: The feature maps output by the CNN branch are globally averaged and pooled to obtain a 128-dimensional feature vector Fcnn; the features of the last frame in the Transformer branch output sequence are extracted into a 64-dimensional feature vector Ftrans; Fcnn and Ftrans are concatenated along their feature dimensions to obtain a 192-dimensional joint feature Fconcat; simultaneously, the 2-dimensional temporal quantization features of the current frame are mapped to a 64-dimensional feature vector Ftime through a fully connected layer with 64 neurons using ReLU activation; Fconcat and Ftime are concatenated along their feature dimensions to form a 256-dimensional final fused feature vector Ffinal; the fused feature vector Ffinal is then fed into a multi-task output head composed of fully connected layers.

[0027] Classification Head: Outputs ground type using a five-class Softmax classifier. The categories include "flat roads, soft soil, gravel, furrows, and grassland";

[0028] First step in regression analysis: A fully connected layer followed by a sigmoid activation function outputs the vibration disturbance level. Constrain its range to [0,1];

[0029] Regression 2: Using the high-frequency energy ratio extracted from the frequency domain features as input, and passing through a fully connected layer and a Sigmoid activation function, the laser positioning degradation risk coefficient is calculated. ;

[0030] Steps 2-3) Integrated Decision Output: Through a multi-task output head, structured decision triples are obtained in parallel. , , ); where the laser positioning degradation risk coefficient The calculation formula is:

[0031] In the formula, This represents the Sigmoid activation function. Represents the ReLU activation function. , and , For the weights and biases of the fully connected layer, Indicates high-frequency vibrational energy. This represents the total vibrational energy across the entire frequency range;

[0032] Steps 2-4) Issuance of decision instructions: The decision triplet is converted into instructions that the positioning system can execute, which serve as the basis for subsequent point cloud adaptive processing and tight coupling state optimization.

[0033] Furthermore, step 3) includes:

[0034] Step 3-1) Decision-driven adaptive parameter matching: Receive the ground type output from Step 2). Vibration disturbance level The index is used to dynamically match the data in a pre-established multi-dimensional mapping library of "ground type-vibration interference level-processing strategy" to extract the corresponding interpolation order and filtering coefficients for the current scenario; the mapping library stores the correlation between the interpolation order and filtering coefficients for different combinations of ground type and vibration interference level.

[0035] Step 3-2) Adaptive Distortion Correction of Point Cloud: Based on the interpolation order obtained through matching, and combined with IMU pre-integration data, motion compensation is performed on the original point cloud of the LiDAR. The laser points at different times within the scanning cycle are uniformly projected to the reference time at the end of the frame. The adaptive distortion correction model is as follows:

[0036]

[0037] In the formula, The corrected point cloud coordinates, The original point cloud coordinates, For the first The pose transformation matrix of each laser point at the acquisition time. The pose transformation matrix at the end of the scan is dynamically calculated based on the interpolation order obtained by matching.

[0038] Step 3-3) Vibration noise suppression and feature preservation: Based on the filter coefficients obtained from matching, local geometric feature evaluation and adaptive statistical filtering are performed on the corrected point cloud. The noise removal judgment formula is as follows:

[0039]

[0040] In the formula, The average distance from the current point to its local neighborhood. The mean of the global point cloud average distance. The risk factor for laser positioning degradation. Standard deviation For step 3-1), match the output filter coefficients of the strategy library;

[0041] Steps 3-4) Point cloud output: Remove outliers identified as noise, aggregate point clouds that retain effective geometric features, and finally output a distortion-free and noise-free point cloud as the laser observation input for backend tight coupling state optimization.

[0042] Furthermore, step 4) includes:

[0043] Step 41) Dynamic switching of the filtering frame: Receive the distortion-free and noise-free point cloud and IMU pre-integration data output from Step 3), and the laser positioning degradation risk coefficient output from Step 2. A dual-filter architecture of multi-state constrained Kalman filter (MSCKF) and iterative error state Kalman filter (IESKF) is constructed, and the degradation risk coefficient of laser positioning is compared. With the preset degradation threshold, the dynamic switching optimization framework, when When the degradation threshold is exceeded, the system switches to the MSCKF framework to enhance system robustness; otherwise, the IESKF framework is maintained to ensure positioning accuracy.

[0044] Step 4-2) Adaptive adjustment of observation weights: Based on the vibration disturbance level output in Step 2). Dynamically adjust the observation weights of lidar , The calculation formula is:

[0045]

[0046] In the formula, As a preset lower limit, This represents the preset weight adjustment coefficient. Indicates the level of vibration interference; The higher, The lower;

[0047] Step 4-3) Post-distortion uncertainty modeling and truncated singular value decomposition (SVD) processing: Introduce a point cloud post-distortion uncertainty model and construct a state error matrix that integrates vibration and noise covariance; During the state iteration optimization process, perform SVD processing on the Jacobian matrix and discard singular value components that are less than a preset threshold.

[0048] Step 4-4) Pose calculation and residual output: Based on the dynamically switched filtering framework and the adjusted observation weights, the optimal state estimation is performed, and the real-time pose estimation result of the mobile carrier is output. At the same time, the pose estimation residual data is extracted and transmitted to step 5) to provide an optimization target for the end-to-end closed-loop feedback.

[0049] Furthermore, step 5) includes:

[0050] Step 5-1) Residual statistical feature extraction and trigger judgment: Receive the pose estimation residual data output in step 4), calculate the root mean square error and variance statistical features of the residual sequence within the current time window; evaluate the stability of the current positioning system by comparing the statistical features with the preset convergence threshold; when the residual index exceeds the convergence threshold, trigger the end-to-end parameter adaptive adjustment mechanism.

[0051] Step 5-2) Dynamic Calculation of Nonlinear Feedback Gain: Based on the statistical characteristics of the residual sequence, a closed-loop feedback gain model is constructed to suppress oscillations and overshoot during parameter adjustment; the formula for calculating the feedback gain coefficient is:

[0052]

[0053] In the formula, Indicates the feedback gain coefficient. This indicates the preset maximum parameter adjustment step size coefficient. The L2 norm of the pose estimation residual vector is represented by... This represents a smoothing factor to prevent frequent parameter jitter when the residual approaches zero; the larger the residual, the greater the feedback gain and the faster the parameter adjustment speed.

[0054] Step 5-3) Front-end multi-stage parameter linkage reconstruction: Based on the calculated feedback gain coefficient and combined with the pre-established residual-parameter sensitivity mapping relationship, generate parameter adaptive adjustment instructions for steps 1) and 3); the adjustment instructions specifically include dynamically modifying the Fast Fourier Transform (FFT) method in step 1) and the filter coefficients and interpolation order in step 3).

[0055] Step 5-4) Command issuance and closed-loop convergence monitoring: The parameter adaptive adjustment command is issued to the corresponding front-end link to complete the parameter update, and the trend of the pose estimation residual after the update is continuously monitored; if the residual does not converge to the threshold, the reverse decay feedback gain coefficient is used for secondary correction until the positioning system achieves continuous self-optimization of the entire link in the dynamically changing ground environment.

[0056] Compared with the prior art, the beneficial effects of the present invention are as follows: The present invention adopts a multi-dimensional standardized ground vibration fingerprint plus a CNN-Transformer network model to realize integrated decision-making on ground type identification and positioning risk, which solves the problem of disconnect between ground perception and positioning decision-making, thereby providing accurate pre-decision basis for the adaptive adjustment of subsequent positioning strategies;

[0057] This invention is based on V level By dynamically adjusting the interpolation order and filter coefficients, the inaccuracy of point cloud correction under high vibration scenarios is solved, and a dynamic balance between point cloud distortion correction and feature preservation is achieved, providing high-quality point cloud input for the backend.

[0058] This invention constructs a dual-filter architecture of MSCKF and IESKF, dynamically switches the filtering strategy and adjusts the observation weights to solve the complex ground positioning drift problem, and achieves reliable pose estimation with high accuracy in conventional scenarios and high robustness in degraded scenarios.

[0059] This invention introduces closed-loop feedback of pose estimation residuals, dynamically adjusts front-end parameters across the entire link, and enables the system to autonomously adapt and continuously optimize. This overcomes the defect of unidirectional error accumulation in open-loop architecture and ensures the long-term operating accuracy of the system in dynamically changing environments. Attached Figure Description

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

[0061] Figure 1 This is a schematic diagram of the vibration sensing and intelligent decision-making process of the present invention.

[0062] Figure 2 This is a schematic diagram of the point cloud adaptive processing and tightly coupled state optimization process of the present invention.

[0063] Figure 3 This is a schematic diagram of the end-to-end closed-loop adaptive optimization process of the present invention. Detailed Implementation

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

[0065] The following explanation uses the industrial-grade IMU module (default output frequency 200 Hz) and lidar (default scanning frequency 10 Hz) mounted on a typical intelligent spraying robot platform as examples.

[0066] like Figure 1 The method for ground-based adaptive lidar-inertial tight-coupled positioning of intelligent spraying robots based on IMU vibration spectrum sensing includes the following steps:

[0067] Step 1) Collect raw IMU data and preprocess it. To avoid the inability of a single dimension feature to fully characterize the ground vibration state, the frequency domain power spectral density (PSD) feature is extracted by Fast Fourier Transform (FFT), and combined with the time domain quantization features (including the root mean square of triaxial composite acceleration and the root mean square of triaxial composite angular velocity) to construct a multidimensional standardized ground vibration fingerprint.

[0068] Step 1-1) Data Acquisition and Preprocessing: Simultaneously acquire raw triaxial acceleration and triaxial angular velocity data from the mobile carrier's IMU. After zero-bias removal, filtering, and pre-integration, signal segments are extracted using a time-sliding window. Specifically, the raw IMU data is transmitted to the system via UART serial port or ROS topic and stored sequentially in a fixed-length 2.0s circular buffer. For each received frame, the timestamp increment is verified. If a timestamp rolls back, the current frame is discarded while retaining existing data in the buffer. If more than 5 consecutive frames roll back, the buffer is cleared and reinitialized. The system extracts the latest continuous data from the buffer at a 10 Hz trigger frequency as the current processing window. The default window length is initialized to 1.28s (this length parameter is dynamically updated by the feedback instruction in Step 5). After subtracting the static zero bias (average of samples taken during the 3-second standstill at system startup) from the window data, it is fed into a Butterworth low-pass filter with a cutoff frequency of 42 Hz to filter out high-frequency harmonic interference from the motor and electronic noise from the sensor. The preprocessed signal is then transmitted to Steps 1-2 and 1-3.

[0069] Steps 1-2) Frequency Domain Feature Extraction: To capture local frequency response differences on the ground, a Fast Fourier Transform (FFT) is performed on the vibration signal within the window to extract the power spectral density (PSD) features, which serve as the frequency domain input for ground type identification. Specifically, a Hanning window is applied to each axis's acceleration signal after preprocessing to suppress spectral leakage, and a 256-point FFT operation is performed (based on a 200 Hz sampling rate, the frequency domain resolution is approximately 200 / 256 ≈ 0.78 Hz / frequency point). The first 128 frequency points (corresponding to the 0~100 Hz effective frequency band) of the single-sided PSD of each axis are taken, and the three-axis PSD vectors are horizontally concatenated to form the frequency domain feature vector. This vector directly reflects the distribution of ground excitation energy. For example, the energy is concentrated in the 0-15 Hz range on smooth roads, while resonance peaks appear in the 30-60 Hz range on gravel or ditch roads. After calling the underlying FFT library to complete the calculation, Pass to steps 1-4).

[0070] Steps 1-3) Time-domain feature quantization: Calculate the time-domain quantization features of the signal within the window, including:

[0071]

[0072]

[0073] In the formula, This represents the root mean square of the triaxial composite acceleration. This represents the root mean square of the combined angular velocity of the three axes. Indicates the number of data points within the window. They represent Triaxial acceleration measurements at time t, They represent Measurements of the triaxial angular velocity at any given time. (During actual engineering measurements) The value ranges from 0.5 to 3.0, with a higher value indicating a more severe ground impact. This characterizes the intensity of the carrier's rotational vibration, reflecting the degree of attitude disturbance induced by ground excitation. After calculation, it will... , Pass to steps 1-4).

[0074] Steps 1-4) Generation of multidimensional standardized ground vibration fingerprints: To eliminate dimensional differences and facilitate downstream fusion, the... Perform L2 norm normalization, and then... , Perform sliding window Min-Max normalization, and the normalized frequency domain features and time domain quantization features together constitute a multidimensional normalized ground vibration fingerprint; specifically, the output of steps 1-2) Perform L2 normalization to preserve the relative strength of each frequency point; then convert the output of steps 1-3) to... , Sliding window Min-Max normalization is used to maintain the historical maxima of the past 30 windows. and minimum value According to the formula Map to the [0,1] interval. If the historical data is less than 30 windows, the factory-calibrated default extreme value is used. The L2-normalized 384-dimensional spectrum vector and the normalized 2-dimensional time-domain quantization feature are encapsulated into independent data structures and passed to step 2-1 as two independent data streams.

[0075] Step 2) Receive the multidimensional standardized ground vibration fingerprint output from Step 1). To overcome the limitations of single-model feature extraction, a pre-trained lightweight convolutional neural network (CNN) and a Transformer network model for capturing long-sequence signal dependencies are used to achieve integrated decision-making for ground state perception and positioning risk assessment. The output includes ground type information. Vibration interference level and laser positioning degradation risk coefficient Structured decision triples;

[0076] Step 2-1) Multidimensional Standardized Ground Vibration Fingerprint Data Access: Receive the 384-dimensional spectral vector and 2-dimensional temporal quantization features output from Step 1-4, and process them in parallel in two paths: The first path directly reshapes the 384-dimensional spectral vector into a spectral tensor with a shape of (1, 1, 384), representing a sequence of 384 frequency points in one batch, one feature channel, which serves as the model input for the CNN branch; the second path maintains a temporal quantization feature queue of fixed length T=10. Upon receiving a frame, the current 2-dimensional temporal quantization feature is pushed to the tail of the queue, and the most recent T frames are extracted to construct the Transformer input sequence with a shape of (1, T, 2). The two model inputs are sent to the lightweight neural network inference engine via ROS custom messages or shared memory. The inference thread runs independently of the main filtering thread. If the inference queue is blocked for more than 50 ms, the spectral tensor and temporal quantization feature queue cached in the previous frame are activated to ensure system continuity, and the effective model input is passed to Step 2-2).

[0077] Step 2-2) Hybrid Model Inference: To address the limitations of single-model feature extraction, the two feature paths output from Step 2-1) are fed into a pre-trained lightweight CNN-Transformer hybrid model, the specific dual-branch architecture of which is described in the invention description. The training process of this hybrid model is as follows:

[0078] Dataset Construction: Using an intelligent spraying robot, raw vibration data was collected with a 200 Hz IMU on five types of ground: smooth roads, soft soil, gravel, furrows, and grassland. Multidimensional standardized ground vibration fingerprints were generated as training samples. At least 5000 frames were collected for each ground type under different operating speeds and load conditions, totaling at least 25000 frames. During training, Gaussian noise (standard deviation 0.02), random scaling of the spectral amplitude (0.9~1.1 times), and random masking of temporal features (10% probability) were applied online to augment the data and improve the model's generalization ability.

[0079] Label generation: Each frame of samples is simultaneously labeled with three types of labels, including ground type. The manual annotation was converted to one-hot encoding; the vibration interference level was calculated based on the current window. and The normalized mean value on the entire dataset is used as a continuous value label for the comprehensive vibration disturbance level, with a value range of [0,1]. By calculating the high-frequency energy ratio / After being correlated and calibrated with the normalized increment of the laser point cloud ICP registration residual relative to the static reference, it is mapped to the [0,1] interval;

[0080] Training strategy: End-to-end training is performed using joint multi-task loss. Cross-entropy loss is used for classification tasks, and mean squared error loss is used for regression tasks. The total loss is a weighted sum of the three. A dynamic weighting strategy based on task uncertainty is used to automatically balance the gradient magnitudes of each task, preventing any one task from dominating the training. The optimizer is AdamW, with an initial learning rate of 1×10⁻⁶. -3 Weight decay 1×10 -4 The batch size was 64, the training lasted 120 epochs, the learning rate was smoothly decayed using a cosine annealing strategy, and an early stopping mechanism was set to prevent overfitting. After training, the model achieved a classification accuracy of no less than 96% on the test set, and the mean absolute errors for the two regression tasks were less than 0.05 and 0.08, respectively.

[0081] Model deployment: After training convergence, the weights are compressed to INT8 precision through post-training quantization, keeping the model size within 1.2MB. End-to-end inference time on the embedded inference unit is less than 8ms. Deployment employs a double-buffered hot update mechanism, automatically rolling back to preset safety parameters in case of inference anomalies to ensure continuous and reliable system operation.

[0082] After completing the inference, the model outputs... , as well as This is passed to steps 2-3) for adaptive adjustment of the subsequent positioning strategy;

[0083] Steps 2-3) Integrated decision output: Generated in parallel through the model's multi-task output heads. , as well as Integrate the three into unified decision-making information: For composite indexes, To establish a continuous correction factor, a structured decision triplet is constructed, allowing ground type and vibration level to jointly determine the foundation treatment strategy. As a continuous adjustment factor, the subsequent filtering weights and point cloud processing parameters are scaled with fine granularity, and the three work together to form a complete decision representation covering discrete classification and continuous risk. The calculation formula is:

[0084]

[0085] In the formula, This represents the Sigmoid activation function. Represents the ReLU activation function. , and , For offline training, fixed weights and biases of fully connected layers are used. It is obtained by summing the elements corresponding to frequencies above 20Hz in the 384-dimensional spectrum vector from steps 1-4). It is obtained by summing all elements of the 384-dimensional spectral vector; this coefficient directly reflects the potential for high-frequency jitter to disrupt the collimation of the laser beam and the geometric continuity of the point cloud; after fusion, the structured decision triple is passed to steps 2-4).

[0086] Steps 2-4) Issuance of Decision Instructions: This includes... , and The decision information is transformed into executable instructions for the positioning system, serving as the dual decision-making basis for subsequent point cloud adaptive processing and tightly coupled state optimization; specifically, the structured decision triples output in steps 2-3) are transformed into instructions: and Discretized values ​​(to Divide the intervals evenly into 5 intervals with a step size of 0.2: [0,0.2), [0.2,0.4), [0.4,0.6), [0.6,0.8), [0.8,1.0]. Take the midpoint of each interval as the discretized value. The value is a composite key, that is, the key value is... In the pre-established "Ground Type - Vibration Disturbance Level - Processing Strategy" mapping library, the corresponding interpolation order and filtering coefficients are extracted from the table, and simultaneously... As a continuous correction factor, the filter coefficients obtained from the lookup table and the subsequent observation weights are linearly scaled according to a preset ratio, ultimately generating an executable instruction package containing three types of parameters: interpolation order, filter coefficients, and observation weight correction factor, which is then encapsulated as a structure. The data is published to the point cloud processing nodes and state estimation nodes via ROS service or shared memory. If no subscribed node is detected, a waiting and retry loop is entered. Step 3) is triggered after confirming that the downstream node is ready.

[0087] like Figure 2 As shown, after obtaining the decision instruction, adaptive point cloud processing and tightly coupled state optimization are performed. Based on the decision result of step 2), the strategy library is dynamically matched to complete the adaptive distortion correction and vibration compensation of the point cloud, providing a high-quality point cloud for the backend state optimization.

[0088] Step 3) Based on the decision triplet output in Step 2), dynamically match the pre-established multi-dimensional mapping library of "ground type-vibration interference level-processing strategy" to extract the interpolation order and filtering coefficients corresponding to the current scene, so as to avoid the fixed parameters causing inaccurate correction under high vibration or over-filtering under low vibration. Accordingly, complete the adaptive distortion correction and vibration compensation of the point cloud, and output the point cloud with vibration noise and distortion removed.

[0089] Step 3-1) Decision-driven adaptive parameter matching: Receives the output from Step 2-4) and The index is used to dynamically match the data in a pre-established multi-dimensional mapping library of "ground type - vibration disturbance level - processing strategy" to extract the corresponding interpolation order and filtering coefficients for the current scenario. Specifically, the strategy library is stored in memory in the form of a two-dimensional hash table, with the key value being... The table lookup operation is completed within one clock cycle. If a match fails, the default safety parameters (interpolation order = 1, filter coefficient = 1.5) are used. The multidimensional mapping library is specifically as follows:

[0090] When the ground type is flat road surface and the vibration interference level is [0, 0.2), the interpolation order is 1 (linear interpolation) and the filter coefficient is 1.0;

[0091] When the ground type is soft soil and the vibration disturbance level is [0.2, 0.4), the interpolation order is 1 (linear interpolation) and the filter coefficient is 1.2.

[0092] When the ground type is gravel and the vibration disturbance level is [0.4, 0.6), the interpolation order is 3 (third-order B-spline interpolation) and the filter coefficient is 2.0.

[0093] When the ground type is furrow and the vibration disturbance level is [0.6, 0.8), the interpolation order is 3 (third-order B-spline interpolation) and the filtering coefficient is 2.5.

[0094] When the ground type is grassland and the vibration disturbance level is [0.8, 1.0], the interpolation order is 4 (fourth-order B-spline interpolation) and the filtering coefficient is 3.0.

[0095] This mapping library calibrates the interpolation order and filtering coefficients for different ground surfaces through field experiments, ensuring that the parameters match the ground vibration characteristics.

[0096] After a successful match, the interpolation order and filter coefficients are passed to step 32.

[0097] Step 3-2) Adaptive Distortion Correction of Point Cloud: Based on the interpolation order obtained through matching, motion compensation is performed on the original point cloud of the lidar in conjunction with the IMU pre-integration data. Laser points at different times within the scanning cycle are uniformly projected to the frame-end reference time. Specifically, each frame of the lidar point cloud carries a nanosecond-level timestamp, which is aligned with the IMU data via a hardware synchronization pin (time deviation less than 2 ms). If the hardware synchronization condition is not met, timestamp-based soft synchronization compensation is used: the system time difference between the point cloud and the IMU data is recorded, and linear interpolation correction is performed on the IMU data timestamp. The correction model is as follows:

[0098]

[0099] In the formula, To correct the point cloud coordinates, Original coordinates This is the carrier pose transformation matrix at the end of the frame. Let be the pose matrix at the moment of emission of the i-th laser point. Interpolation is performed using the IMU pre-integration results, directly based on the interpolation order obtained in step 31: when the obtained interpolation order is 1, linear interpolation is used to reduce computational load; when the obtained interpolation order is greater than or equal to 2, to avoid linear interpolation failing to fit high-frequency pose changes, it dynamically switches to higher-order B-spline interpolation of the corresponding order, using multiple adjacent pre-integration poses to construct a smooth curve, avoiding pose jumps caused by sudden changes in IMU zero bias under high-frequency vibrations. After correction, The set is passed to step 33;

[0100] Step 3-3) Vibration noise suppression and feature preservation: Based on the filter coefficients obtained from matching, local geometric feature evaluation and adaptive statistical filtering are performed on the corrected point cloud; specifically, the local neighborhood average distance of each point is calculated. (Neighborhood radius 0.1m, nearest neighbor number 20). The noise removal criterion formula is:

[0101]

[0102] In the formula, The mean distance of the point cloud across the entire frame. The risk factor for laser positioning degradation. Standard deviation The filter coefficients output from the strategy library in step 3-1) dynamically change with the vibration level to avoid incorrectly deleting real structures under high vibration or leaving noise under low vibration due to a fixed threshold. The more severe the vibration, the larger the value and the wider the threshold, to avoid misjudging real structures as noise. For smooth road surfaces, the value is smaller and the threshold is tightened to preserve edge details. After completing the removal by calling the point cloud library filtering interface, the effective point set is passed to step 3-4).

[0103] Steps 3-4) High-quality point cloud output: Outliers identified as noise are removed, and point clouds retaining effective geometric features are aggregated to finally output a distortion-free and noise-free high-quality point cloud, providing reliable laser observation input for backend tight-coupled state optimization; specifically, the filtered point cloud is recalculated with bounding boxes and voxel downsampling (voxel size 0.05m), the data volume is compressed by approximately 40%, and packaged into... The message is published to the backend topic. Step 4 is triggered after successful publication.

[0104] Step 4) Fuse the vibration-noise- and distortion-free point cloud and IMU pre-integrated data output from Step 3), and based on the laser positioning degradation risk coefficient in the decision triplet output from Step 2). Vibration disturbance level The system dynamically switches between the multi-state constrained Kalman filter (MSCKF) and the iterative error state Kalman filter (IESKF) frameworks, and adjusts the observation weights to avoid positioning drift caused by laser observation under severe vibration, thereby achieving adaptive tightly coupled state optimization and outputting pose estimation residuals.

[0105] Step 4-1) Dynamic switching of the filtering framework: To balance conventional accuracy and degradation robustness, the high-quality point cloud and IMU pre-integrated data output from step 3, as well as the data output from step 2), are received. Construct a dual-filter architecture combining MSCKF and IESKF, and compare the results. The system dynamically switches between two optimization frameworks based on a preset degradation threshold. To ensure state compatibility between the two frameworks, the system uniformly adopts a sliding window state vector definition that includes the current pose, the inertial measurement unit (IMU) zero-bias estimate, and the poses of the last 5 frames. MSCKF and IESKF share the same state structure. In IESKF mode, the historical pose is treated as a constant in the optimization process but not updated, while in MSCKF mode, all poses within the window are jointly updated. The system maintains two filter instances in parallel in the background, sharing the same IMU pre-integration queue. (Preset degradation threshold) (Obtained through field experiments under different vibration scenarios). When When, activate the IESKF thread to perform normal distribution transformation / iterative nearest point direct pose optimization; when To prevent iterative optimization from diverging due to degenerate observations, the historical pose window in the state vector is used as a feature-associated frame and routed to the MSCKF thread for multi-state constraint updates. During switching, the state prediction queue remains continuous, and the historical pose window is filled from the keyframe cache maintained in IESKF mode. After completion, the handle of the currently active frame is passed to step 4-2.

[0106] Step 4-2) Adaptive adjustment of observation weights: based on Dynamic adjustment The calculation formula is: in, For nominal observation weights, This is a preset lower limit (to prevent the weight from approaching zero when the vibration level is too high, which would cause the system to drift). This is the sensitivity coefficient. When the... At higher levels, the aforementioned The lower it is adjusted, the more it passes the lower limit. Ensure that it still has no less than [a certain value] under the worst vibration conditions. The observation confidence level is adjusted to prevent the system from diverging due to complete reliance on the prediction model. After the weights are adjusted, the results are passed to step 4-3.

[0107] Step 4-3) Post-distortion uncertainty modeling and SVD processing: To suppress the negative impact of degraded observations on the state, a point cloud post-distortion uncertainty model is introduced, and a state error matrix integrating vibration noise covariance is constructed. During the state iterative optimization process, the Jacobian matrix is ​​processed by SVD, discarding singular value components smaller than a preset threshold (set to 10% of the maximum singular value). Specifically, if the point cloud features degrade during IESKF iterative solution, the Jacobian matrix is ​​prone to ill-conditioned behavior. SVD is performed on the Jacobian matrix, forcibly setting singular value components smaller than 0.01 to zero before reconstructing the matrix. This operation eliminates degraded direction observations that cause numerical instability, retaining strong constraint features. The truncated SVD is implemented using a linear algebra library, with a single solution taking less than 2ms. After completion, the stabilized Jacobian matrix is ​​passed to step 4-4).

[0108] Step 4-4) Pose Calculation and Residual Output: Based on the dynamically switched filtering framework and adjusted observation weights, optimal state estimation is performed, outputting high-precision real-time pose estimation results for the moving vehicle, and simultaneously extracting pose estimation residual data; specifically, the filter output includes the optimal state vector containing position, velocity, attitude quaternions, gyroscope bias, and accelerometer bias, while simultaneously calculating the pose estimation residual vector between the actual and predicted observations (denoted as...). The residual sequence is stored in a sliding queue of length 15, and the pose results are published as an odometry topic. Step 5) is triggered after confirming that the data output is normal.

[0109] like Figure 3 As shown, in order to achieve continuous self-optimization of the system, full-link closed-loop adaptive optimization is performed.

[0110] Step 5) Based on the pose estimation residuals output in Step 4), assess the degree of mismatch between the current positioning system parameters and the environment, overcome the defect of unidirectional error accumulation in the open-loop architecture, and provide feedback to adjust the front-end parameters in Step 1) and Step 3) to correct error accumulation and achieve end-to-end closed-loop adaptive optimization.

[0111] Step 5-1) Residual Statistical Feature Extraction and Trigger Judgment: To assess the mismatch between system parameters and the dynamic environment, the pose estimation residual data output in Step 4-4 is received, and the root mean square error and variance statistical features of the residual sequence within the current time window are calculated. By comparing the statistical features with a preset convergence threshold, the stability of the current positioning system is assessed. Specifically, the root mean square error (RMSE) of the residuals in the most recent 15 frames (denoted as RMSE) is calculated. , ,in (residual L2 norm) and variance (denoted as) ).like or If the current parameter set is mismatched with the environment, set the adaptive trigger flag; otherwise, if the system is stable, skip the feedback of this cycle and return directly to step 1) to wait for the next round of data input.

[0112] Step 5-2) Dynamic Calculation of Nonlinear Feedback Gain: Based on the statistical characteristics of the residual sequence, a closed-loop feedback gain model is constructed to suppress oscillations and overshoot during parameter adjustment; specifically, the formula for calculating the feedback gain coefficient is:

[0113]

[0114] In the formula, For feedback gain coefficient, The maximum adjustment step size is preferred (0.12). The anti-shake smoothing factor is set to 0.001 (preferably). This function ensures smoothness even with large residuals. Fast adjustment is achieved by rapidly approaching the maximum value, and fine adjustment is achieved by smoothly converging to 0 when the residual is small. After the calculation is completed, the result is passed to step 5-3.

[0115] Step 5-3) Front-end multi-stage parameter linkage reconstruction: To correct the error accumulation caused by front-end parameter mismatch, based on the calculated feedback gain coefficient, combined with the pre-established residual-parameter sensitivity mapping relationship (when... In step 1), an overlapping FFT window technique is used (maintaining a window length of 1.28 s but increasing the overlap rate by 50%) to improve the spectral response speed. Simultaneously, the interpolation order and filter coefficients in step 3) are adjusted (the interpolation order is increased by 1 order (up to an upper limit of 4), and the filter coefficients are increased by 0.2). ;when Then, adjust the filter coefficients in step 3) by increasing them by 0.5 from the original values. To suppress vibration noise; if both exceed the threshold, the interpolation order is increased by 1 (up to 4), and the filter coefficient is increased by 0.5. The system generates adaptive parameter adjustment instructions for steps 1) and 3). These instructions are distributed via the ROS parameter server and automatically take effect on each node in the next processing cycle without requiring a restart. After generation, the instructions are passed to steps 5-4).

[0116] Step 5-4) Command Issuance and Closed-Loop Convergence Monitoring: The command is issued to the corresponding front-end stage to complete the parameter update, and the residual trend is continuously monitored. If convergence is not achieved, a second correction is made to ensure that the end-to-end closed-loop self-optimization is effective. Specifically, after issuance, a 500 ms monitoring window is entered. If the residual in the next 5 frames not only fails to converge but also shows an increasing trend, it is determined that the current parameter adjustment direction is incorrect, and a reverse adjustment is performed (changing the original interpolation order from increasing to decreasing, and the filter coefficient from increasing to decreasing), and the feedback gain coefficient is reduced. If the residual converges but still exceeds the threshold, the direction is considered correct but the step size is too large, and only execution is performed. The system continues to adjust in the same direction until the pose estimation residual falls within the convergence threshold. Then, it exits parameter adjustment, resumes the normal positioning process, and the positioning system achieves continuous self-optimization across the entire link in a dynamically changing ground environment.

[0117] The above description of the embodiments is only for the purpose of helping to understand the method and core ideas of the present invention. It should be noted that those skilled in the art can make several improvements and modifications to the present invention without departing from the principles of the present invention, and these improvements and modifications also fall within the protection scope of the claims of the present invention.

Claims

1. A tightly coupled inertial positioning method for a spraying robot based on IMU vibration spectrum sensing using lidar, characterized in that, Includes the following steps: Step 1) Collect raw IMU data and preprocess it. Extract the frequency domain power spectral density (PSD) features through Fast Fourier Transform (FFT) and construct a multidimensional standardized ground vibration fingerprint by combining the time domain quantization features. Step 2) Receive the multidimensional normalized ground vibration fingerprint output from Step 1). Utilize a pre-trained lightweight convolutional neural network (CNN) and a Transformer network model for capturing long-sequence signal dependencies to achieve integrated decision-making for ground state perception and positioning risk assessment. The output includes ground type information. Vibration interference level and laser positioning degradation risk coefficient Structured decision triples; Step 3) Based on the decision triplet output in Step 2), dynamically match the pre-established multi-dimensional mapping library of "ground type-vibration interference level-processing strategy", extract the interpolation order and filtering coefficients corresponding to the current scene, and complete the adaptive distortion correction and vibration compensation of the point cloud accordingly, and output the point cloud with vibration noise and distortion removed. Step 4) Fuse the vibration-noise- and distortion-free point cloud and IMU pre-integrated data output from Step 3), and based on the laser positioning degradation risk coefficient in the decision triplet output from Step 2). Vibration disturbance level The system dynamically switches between the multi-state constrained Kalman filter (MSCKF) and the iterative error state Kalman filter (IESKF) frameworks, and adjusts the observation weights to avoid positioning drift caused by laser observation under severe vibration, thereby achieving adaptive tightly coupled state optimization and outputting pose estimation residuals. Step 5) Based on the pose estimation residuals output in Step 4), assess the degree of mismatch between the current positioning system parameters and the environment, and provide feedback to adjust the front-end parameters in Steps 1) and 3) to correct the accumulated error and achieve end-to-end closed-loop adaptive optimization.

2. The spraying robot lidar inertial tight-coupled positioning method based on IMU vibration spectrum sensing according to claim 1, characterized in that, Step 1) includes: Step 1-1) Data Acquisition and Preprocessing: Synchronously acquire raw IMU data, and after zero bias removal, filtering, and pre-integration, extract signal segments through a time sliding window; Steps 1-2) Frequency domain feature extraction: In order to capture the local frequency response differences of the ground, the vibration signal within the window is subjected to Fast Fourier Transform (FFT) to extract the frequency domain power spectral density (PSD) features, which are used as the frequency domain input for ground type identification; Steps 1-3) Time-domain feature quantization: Calculate the time-domain quantization features of the signal within the window, including the root mean square of the triaxial composite acceleration and the root mean square of the triaxial composite angular velocity. The calculation formula is as follows: ； ； In the formula, This represents the root mean square of the triaxial composite acceleration. This represents the root mean square of the combined angular velocity of the three axes. Indicates the number of data points within the window. These represent the triaxial acceleration measurements at time k. These represent the triaxial angular velocity measurements at time k; the root mean square value is used to quantify the ground vibration intensity, with larger values ​​indicating more severe vibration. Steps 1-4) Generation of Multidimensional Normalized Ground Vibration Fingerprint: The frequency domain power spectral density (PSD) features are normalized using the L2 norm to preserve the relative strength relationships at each frequency point; and Sliding window Min-Max normalization is performed to map the frequency domain features to the [0,1] interval; the normalized frequency domain features and time domain quantization features together constitute a multidimensional standardized ground vibration fingerprint.

3. The spraying robot lidar inertial tight-coupled positioning method based on IMU vibration spectrum sensing according to claim 1, characterized in that, Step 2) includes: Step 2-1) Multidimensional Normalized Ground Vibration Fingerprint Data Access: Receive the multidimensional normalized ground vibration fingerprint generated in Step 1), shape the normalized frequency domain features into a spectral tensor, and use it as input to the CNN branch; simultaneously, maintain a fixed-length T-frame temporal quantization feature queue, and store the temporal quantization features of the most recent T frames (…). , This is constructed as a time series and used as input for the Transformer branch; Step 2-2) Hybrid Model Inference: Input the spectral features and temporal quantization features into a pre-trained lightweight CNN-Transformer hybrid model. The specific structure of the hybrid model is as follows: The Convolutional Neural Network (CNN) branch consists of three one-dimensional convolutional layers, each followed by a max-pooling layer. This branch receives a spectral tensor and is used to extract local frequency response patterns that cannot be represented by single time-domain statistics. Transformer branch: Consists of two stacked Transformer encoders, each containing a multi-head self-attention mechanism and a feedforward network; this branch receives a queue of temporal quantized features to capture long-range cumulative trends and dependencies that are easily ignored by single frequency domain features; Feature fusion and multi-task output head: The feature maps output by the CNN branch are globally averaged and pooled to obtain a 128-dimensional feature vector Fcnn; the features of the last frame in the Transformer branch output sequence are extracted into a 64-dimensional feature vector Ftrans; Fcnn and Ftrans are concatenated along their feature dimensions to obtain a 192-dimensional joint feature Fconcat; simultaneously, the 2-dimensional temporal quantization features of the current frame are mapped to a 64-dimensional feature vector Ftime through a fully connected layer with 64 neurons using ReLU activation; Fconcat and Ftime are concatenated along their feature dimensions to form a 256-dimensional final fused feature vector Ffinal; the fused feature vector Ffinal is then fed into a multi-task output head composed of fully connected layers. Classification Head: Outputs ground type using a five-class Softmax classifier. The categories include "flat roads, soft soil, gravel, furrows, and grassland"; First step in regression analysis: A fully connected layer followed by a sigmoid activation function outputs the vibration disturbance level. Constrain its range to [0,1]; Regression 2: Using the high-frequency energy ratio extracted from the frequency domain features as input, and passing through a fully connected layer and a Sigmoid activation function, the laser positioning degradation risk coefficient is calculated. ; Steps 2-3) Integrated Decision Output: Through a multi-task output head, structured decision triples are obtained in parallel. , , ); where the laser positioning degradation risk coefficient The calculation formula is: In the formula, This represents the Sigmoid activation function. Represents the ReLU activation function. , and , For the weights and biases of the fully connected layer, Indicates high-frequency vibrational energy. This represents the total vibrational energy across the entire frequency range; Steps 2-4) Issuance of decision instructions: The decision triplet is converted into instructions that the positioning system can execute, which serve as the basis for subsequent point cloud adaptive processing and tight coupling state optimization.

4. The spraying robot lidar inertial tight-coupled positioning method based on IMU vibration spectrum sensing according to claim 1, characterized in that, Step 3) includes: Step 3-1) Decision-driven adaptive parameter matching: Receive the ground type output from Step 2). Vibration disturbance level The data is used as an index to perform dynamic matching in a pre-established multi-dimensional mapping library of "ground type-vibration interference level-processing strategy" to extract the corresponding interpolation order and filtering coefficients for the current scenario; the mapping library stores the correlation between the interpolation order and filtering coefficients for different combinations of ground type and vibration interference level. Step 3-2) Adaptive Distortion Correction of Point Cloud: Based on the interpolation order obtained through matching, and combined with IMU pre-integration data, motion compensation is performed on the original point cloud of the LiDAR. The laser points at different times within the scanning cycle are uniformly projected to the reference time at the end of the frame. The adaptive distortion correction model is as follows: ； In the formula, The corrected point cloud coordinates, The original point cloud coordinates, For the first The pose transformation matrix of each laser point at the acquisition time. The pose transformation matrix at the end of the scan is dynamically calculated based on the interpolation order obtained by matching. Step 3-3) Vibration noise suppression and feature preservation: Based on the filter coefficients obtained from matching, local geometric feature evaluation and adaptive statistical filtering are performed on the corrected point cloud. The noise removal judgment formula is as follows: ； In the formula, The average distance from the current point to its local neighborhood. The mean of the global point cloud average distance. The risk factor for laser positioning degradation. Standard deviation For step 3-1), match the output filter coefficients of the strategy library; Steps 3-4) Point cloud output: Remove outliers identified as noise, aggregate point clouds that retain effective geometric features, and finally output a distortion-free and noise-free point cloud as the laser observation input for backend tight coupling state optimization.

5. The spraying robot lidar inertial tight-coupled positioning method based on IMU vibration spectrum sensing according to claim 1, characterized in that, Step 4) includes: Step 41) Dynamic switching of the filtering frame: Receive the distortion-free and noise-free point cloud and IMU pre-integration data output from Step 3), and the laser positioning degradation risk coefficient output from Step 2. A dual-filter architecture of multi-state constrained Kalman filter (MSCKF) and iterative error state Kalman filter (IESKF) is constructed, and the degradation risk coefficient of laser positioning is compared. With the preset degradation threshold, the dynamic switching optimization framework, when When the degradation threshold is exceeded, the system switches to the MSCKF framework to enhance system robustness; otherwise, the IESKF framework is maintained to ensure positioning accuracy. Step 4-2) Adaptive adjustment of observation weights: Based on the vibration disturbance level output in Step 2). Dynamically adjust the observation weights of lidar , The calculation formula is: ； In the formula, As a preset lower limit, This represents the preset weight adjustment coefficient. Indicates the level of vibration interference; The higher, The lower; Step 4-3) Post-distortion uncertainty modeling and truncated singular value decomposition (SVD) processing: Introduce a point cloud post-distortion uncertainty model and construct a state error matrix that integrates vibration and noise covariance; During the state iteration optimization process, perform SVD processing on the Jacobian matrix and discard singular value components that are less than a preset threshold. Step 4-4) Pose calculation and residual output: Based on the dynamically switched filtering framework and the adjusted observation weights, the optimal state estimation is performed, and the real-time pose estimation result of the mobile carrier is output. At the same time, the pose estimation residual data is extracted and transmitted to step 5) to provide an optimization target for the end-to-end closed-loop feedback.

6. The spraying robot lidar inertial tight-coupled positioning method based on IMU vibration spectrum sensing according to claim 1, characterized in that, Step 5) includes: Step 5-1) Residual statistical feature extraction and trigger judgment: Receive the pose estimation residual data output in step 4), calculate the root mean square error and variance statistical features of the residual sequence within the current time window; evaluate the stability of the current positioning system by comparing the statistical features with the preset convergence threshold; when the residual index exceeds the convergence threshold, trigger the end-to-end parameter adaptive adjustment mechanism. Step 5-2) Dynamic Calculation of Nonlinear Feedback Gain: Based on the statistical characteristics of the residual sequence, a closed-loop feedback gain model is constructed to suppress oscillations and overshoot during parameter adjustment; the formula for calculating the feedback gain coefficient is: ； In the formula, Indicates the feedback gain coefficient. This indicates the preset maximum parameter adjustment step size coefficient. The L2 norm of the pose estimation residual vector is represented by... This represents a smoothing factor to prevent frequent parameter jitter when the residual approaches zero; the larger the residual, the greater the feedback gain and the faster the parameter adjustment speed. Step 5-3) Front-end multi-stage parameter linkage reconstruction: Based on the calculated feedback gain coefficient and combined with the pre-established residual-parameter sensitivity mapping relationship, generate parameter adaptive adjustment instructions for steps 1) and 3); the adjustment instructions specifically include dynamically modifying the Fast Fourier Transform (FFT) method in step 1) and the filter coefficients and interpolation order in step 3). Step 5-4) Command issuance and closed-loop convergence monitoring: The parameter adaptive adjustment command is issued to the corresponding front-end link to complete the parameter update, and the trend of the pose estimation residual after the update is continuously monitored; if the residual does not converge to the threshold, the reverse decay feedback gain coefficient is used for secondary correction until the positioning system achieves continuous self-optimization of the entire link in the dynamically changing ground environment.

Images