An RTK and IMU fusion state estimation method based on particle swarm optimization

Abstract

The application relates to the technical field of navigation positioning, and particularly discloses an RTK and IMU fusion state estimation method based on particle swarm optimization, which comprises the following steps: firstly, raw data in different environments are synchronously collected by using an RTK device and an IMU sensor, and pretreatment operation is performed; a fusion state estimation model is constructed based on an adaptive Kalman filtering algorithm, and fusion parameters are dynamically adjusted; a hybrid optimization algorithm combining particle swarm optimization and local gradient descent is used to perform parameter optimization on the self-fusion state estimation model; core performance indexes of the model are evaluated through a verification set; and the model stability is verified in an actual scene by using a test set. The RTK and IMU fusion state estimation method based on particle swarm optimization has the advantages that the method is suitable for mountainous, high-altitude and sandy environments, can meet the scene requirements of mountain surveying and mapping, wind power inspection and the like, is high in applicability and easy to popularize.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of navigation positioning, in particular to an RTK and IMU fusion state estimation method based on particle swarm optimization. BACKGROUND

[0002] In the field of navigation positioning, the fusion technology of real-time kinematic (RTK) and inertial measurement unit (IMU) is playing an important role. Although RTK technology can provide centimeter-level high-precision positioning when used alone, its positioning performance will decrease significantly or even fail completely in environments such as urban canyons and mountain tunnels where satellite signals are blocked. When working alone, IMU can realize continuous positioning due to its autonomous inertial measurement characteristics, but its positioning error will accumulate over time due to the inherent zero offset and noise of the sensor, making it difficult to meet the demand for long-time high-precision positioning. Due to the limitations of these two technologies, the fusion of the two technologies has been developed, and through complementary advantages, it has been successfully applied in many fields.

[0003] In practical applications, there are also many challenges. For example, in the fixed weight fusion mode used by unmanned aerial vehicle inspection, it can still meet the demand in flat gobi areas, but when entering mountainous or forest areas, the interruption of RTK signals will lead to positioning errors that cannot be effectively compensated. The fusion scheme based on extended Kalman filter used by the automatic driving system cannot adapt to the rapid accumulation of IMU errors in long tunnels and other scenarios because the noise covariance matrix is set with fixed parameters. The loose coupling architecture used in geographic surveying and mapping operations cannot respond to changes in signal quality in time, resulting in deviations in surveying and mapping data.

[0004] The existing fusion technology mainly includes fixed weight fusion and residual-based adaptive fusion, but both have obvious shortcomings in practical applications. The fixed weight fusion method relies on experience to preset coefficients and completely lacks environmental adaptability, and cannot adjust the contribution of sensors according to the real-time changes in RTK signal quality. The residual-based adaptive fusion method can adjust the weight by observing the residual error, but it has two defects: on the one hand, it fails to fully consider the dynamic changes of sensor noise characteristics, especially in complex environments such as sandstorms; on the other hand, its adaptive process needs to accumulate multiple frames of data, resulting in insufficient response speed and obvious lag in fast maneuvering scenarios.

[0005] In addition, the existing technology also has the following limitations: noise parameters are set statically, making it difficult to adapt to changes in complex environments; there is a lack of specialized optimization measures for terrain features such as high altitudes and steep slopes; in scenarios where signals are interrupted for a long time, the error suppression capability is limited, which seriously affects the reliability and positioning accuracy of the system. The existence of these problems seriously restricts the application effect of the fusion technology in complex environments. SUMMARY

[0006] The application aims to provide an RTK and IMU fusion state estimation method based on particle swarm optimization, solve the problems of poor self-adaptability, large positioning error and response lag of the existing fusion technology in complex environments, and realize high-precision and high-stability positioning in different scenes.

[0007] To achieve the above-mentioned purpose, the application provides an RTK and IMU fusion state estimation method based on particle swarm optimization, comprising the following steps:

[0008] S1, synchronously collecting original data in different environments by using an RTK device and an IMU sensor;

[0009] S2, pre-processing the collected original data, including RTK position data denoising and IMU data correction filtering;

[0010] S3, constructing a fusion state estimation model based on an adaptive Kalman filtering algorithm and dynamically adjusting fusion parameters;

[0011] S4, using a hybrid optimization algorithm combining particle swarm optimization and local gradient descent to optimize parameters of the self-fusion state estimation model;

[0012] S5, evaluating core performance indicators of the model through a verification set;

[0013] S6, verifying the stability of the model in actual scenes by using a test set.

[0014] Preferably, in S1, the RTK device is used to collect satellite positioning data and corresponding time stamps The satellite positioning data are original position data including longitude , latitude and height ; at the same time, original three-axis acceleration and original three-axis angular velocity data are synchronously collected by the IMU sensor, and corresponding time stamps are recorded.

[0015] Preferably, in S2, the pre-processing of the collected original data is specifically as follows:

[0016] S21, using a Gaussian filtering algorithm to perform denoising processing on the original position data collected by the RTK device, and the calculation formula is as follows:

[0017] ;

[0018] wherein, is the original position data collected by the RTK device, is a dynamic standard deviation, is a data mean value, to the denoised position data;

[0019] S22, obtain the accelerometer zero offset through static calibration and the gyroscope zero offset , first correct the original three-axis acceleration and three-axis angular velocity data collected by the IMU sensor, the formula is:

[0020] , , ;

[0021] , , ;

[0022] wherein, , , is the corrected acceleration, , , is the corrected angular velocity;

[0023] then use the Butterworth low-pass filter to remove high-frequency noise, and the transfer function is:

[0024] ;

[0025] wherein, is a complex variable, is the cutoff frequency, is the order of the filter.

[0026] Preferably, in S3, a fusion state estimation model is constructed based on an adaptive Kalman filtering algorithm, specifically including the following steps:

[0027] S31, define the system vector under discrete time:

[0028] ;

[0029] wherein, is a three-dimensional position coordinate, is a three-dimensional velocity component, is a three-dimensional attitude angle, is a roll angle, is a pitch angle, is a yaw angle, is a three-dimensional angular velocity component;

[0030] S32, construct a state transition matrix , the expression is as follows:

[0031] ;

[0032] wherein, represents the time interval between two adjacent discrete time points; and a dynamic compensation term is introduced in the state transition matrix;

[0033] S33, constructing a measurement matrix according to the measurement characteristics of RTK and IMU , the expression is as follows:

[0034] ;

[0035] wherein, the first 3 rows correspond to the position measurement of RTK, mapping the position coordinate part in the state vector; the middle 3 rows correspond to the acceleration measurement of IMU, mapping the velocity component part in the state vector; the last 3 rows correspond to the angular velocity measurement of IMU, mapping the angular velocity component part in the state vector; the observation correction factor based on satellite signal-to-noise ratio and height is introduced in the measurement matrix ;

[0036] S34, dynamically adjusting the fusion weight, defining the fusion weight of RTK as , and the fusion weight of IMU as , and the calculation formula is:

[0037] ;

[0038] ;

[0039] wherein, is the satellite signal-to-noise ratio of RTK, is the number of satellites received by RTK, is the IMU zero offset balance coefficient, is the IMU zero offset estimate value, is the IMU error accumulation balance coefficient, is the error accumulation time of IMU;

[0040] The output of the fusion model is:

[0041] ;

[0042] wherein, is the denoised RTK positioning data, is the corrected IMU fusion data;

[0043] S35, dynamically adjusting the process noise covariance and the measurement noise covariance :

[0044] Adding a compensation term of acceleration change rate , the process noise covariance The expression is as follows:

[0045] ;

[0046] wherein, is the initial process noise covariance, is the dynamic compensation coefficient;

[0047] The measurement noise covariance is as follows:

[0048] ;

[0049] wherein, is the residual influence coefficient, is the Kalman filter innovation, is the 2-norm of information; The first block of the diagonal matrix is the RTK measurement noise, wherein, is the RTK measurement noise covariance; the second block is the IMU acceleration measurement noise, is the IMU acceleration measurement noise covariance, is the bias influence coefficient; the third block is the IMU angular velocity measurement noise is the time accumulation influence coefficient, is the angular velocity measurement noise covariance.

[0050] Preferably, in S4, the preprocessed data is randomly divided into a training set, a validation set and a test set in a ratio of 7:2:1, the training set is used to iteratively optimize the model parameters, and the optimization target is to minimize the root mean square of positioning error :

[0051] ;

[0052] wherein, is the total number of training samples, represents the index number of the training sample, and is used to identify the th training sample, is the model output value of the th training sample, is the true positioning value of the th training sample.

[0053] Preferably, in S4, a hybrid optimization algorithm combining particle swarm optimization PSO and local gradient descent is adopted, and sensitive parameters of the model including IMU bias balance coefficient , IMU error accumulation balance coefficient , filter order and cutoff frequency Optimization is performed; in the particle swarm optimization stage, the optimal parameter interval is quickly located through group cooperation, and the specific process is as follows:

[0054] Construct a composite fitness function :

[0055] ;

[0056] wherein, is the root mean square of the positioning error to be minimized; is the standard deviation of the positioning error; is the filter convergence time of the model on the validation set; , , is a weight coefficient;

[0057] Dynamically adjust the inertia weight , the formula is as follows:

[0058] ;

[0059] wherein, is the basic inertia weight; is the average satellite signal-to-noise ratio of the training data subset corresponding to the current particle; is the maximum possible value of the signal-to-noise ratio; is a regulation coefficient;

[0060] According to the historical sensitivity of the parameters to , the maximum speed is constrained:

[0061] ;

[0062] wherein, is a proportional coefficient, is a small amount to prevent division by zero, is the parameter to be optimized;

[0063] The global optimal parameter output by the environment adaptive PSO is taken as the initial point, then the gradient direction is dominated by the integral term of the IMU velocity error:

[0064] ;

[0065] wherein, is the corrected IMU acceleration, is the zero offset estimation value, is the optimal IMU zero offset balance coefficient after optimization, is the optimal IMU error accumulation balance coefficient after optimization. ​​

[0066] Preferably, in S5, the core performance indicators of the model include positioning accuracy, response time and error suppression capability, the positioning accuracy includes plane positioning error and height positioning error, the response time includes weight adjustment response delay and filter convergence time, and the error suppression capability includes long-time signal interruption error and environmental noise suppression rate.

[0067] Preferably, in S5, the plane positioning error is defined as that 95% of the positioning points fall within a circular area centered on the true position, reflecting the positioning accuracy of the model in the horizontal direction.

[0068] The height positioning error is defined as that the absolute value of the error of 95% of the positioning points in the height direction is less than LE95, reflecting the accuracy of the model in the vertical direction.

[0069] Preferably, in S5, the adjustment response delay is defined as the time from the mutation of the RTK signal quality or the mutation of the IMU noise to the stabilization of the fusion weight at the new target value, reflecting the rapid adaptation ability of the model to environmental changes.

[0070] The filter convergence time is defined as the time required for the position item variance of the state covariance matrix to drop to 1x10 -5 m 2 after the model is started or the signal interruption is recovered, reflecting the rapid stabilization ability of the model.

[0071] Preferably, in S5, the long-time signal interruption error is defined as the cumulative amount of positioning error at different lengths of time after the RTK signal is completely interrupted, reflecting the effectiveness of the IMU error suppression algorithm.

[0072] The environmental noise suppression rate is defined as the ratio of the noise variance of the model output data to the noise variance of the original sensor data, reflecting the suppression ability of the model to environmental noise.

[0073] Therefore, the present application adopts the above-mentioned RTK and IMU fusion state estimation method based on particle swarm optimization, and has the following beneficial effects:

[0074] (1) The present application can accurately adapt to environmental characteristics such as large slope, high altitude, sand weather, etc. by dynamically adjusting the Gaussian filter window, the IMU zero bias calibration parameter and the observation correction factor, solving the limitation of one parameter used in all scenarios in the prior art, and significantly improving the applicability.

[0075] (2) In the present application, the plane CEP95 is ≤0.3m, the height LE95 is ≤0.4m, and the long-time signal interruption (180s) error is ≤1.2m, which can meet the high-precision requirements of mountain mapping, wind power inspection and highway construction, and the precision is significantly improved compared with traditional technology.

[0076] (3) The weight adjustment response delay of the present invention is ≤0.12s and the filtering convergence time is ≤3.5s. It can quickly respond to sudden interference such as sandstorm weather and signal blockage, and avoid positioning inaccuracy caused by response lag. At the same time, the noise suppression rate is ≥82%, effectively filtering out sensor noise caused by mountain bumps and low air pressure at high altitudes.

[0077] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0078] Figure 1 This is an overall flowchart of an embodiment of the RTK and IMU fusion state estimation method based on particle swarm optimization according to the present invention.

[0079] Figure 2 This is a schematic diagram of the adaptive Kalman filter fusion process in an embodiment of the state estimation method for RTK and IMU fusion based on particle swarm optimization according to the present invention.

[0080] Figure 3 This is a schematic diagram comparing the trajectory overlap of an embodiment of the RTK and IMU fusion state estimation method based on particle swarm optimization of the present invention with that of traditional methods. Detailed Implementation

[0081] The technical solution of the present invention will be further described below with reference to the accompanying drawings and embodiments.

[0082] Unless otherwise defined, the technical or scientific terms used in this invention shall have the ordinary meaning as understood by one of ordinary skill in the art to which this invention pertains.

[0083] like Figure 1 As shown, a state estimation method based on particle swarm optimization for RTK and IMU fusion includes the following steps:

[0084] S1. Use RTK equipment and IMU sensors to synchronously collect raw data under different environments.

[0085] Specifically, this invention utilizes RTK equipment to collect satellite positioning data and corresponding timestamps. Satellite positioning data includes longitude ,latitude and height raw location data Simultaneously, raw triaxial acceleration is acquired via an IMU sensor. and the original triaxial angular velocity data And record the corresponding timestamp. .

[0086] S2. Preprocess the collected raw data, specifically as follows:

[0087] S21, using a Gaussian filtering algorithm, the raw position data collected by the RTK device is denoised, and the calculation formula is:

[0088] ;

[0089] wherein, is the raw position data collected by the RTK device, is the dynamic standard deviation, is the data mean, is the denoised position data.

[0090] S22, obtain the accelerometer zero offset and the gyroscope zero offset , first, the original three-axis acceleration and three-axis angular velocity data collected by the IMU sensor are corrected for zero offset, and the formula is:

[0091] , , ;

[0092] , , ;

[0093] wherein, , , is the corrected acceleration, , , is the corrected angular velocity.

[0094] Then use the Butterworth low-pass filter to remove high-frequency noise, and its transfer function is:

[0095] ;

[0096] wherein, is a complex variable, is the cutoff frequency, the initial value is 10Hz, is the order of the filter, the initial value is set to 4 orders.

[0097] As Figure 2 shown, S3, based on the adaptive Kalman filtering algorithm, a fusion state estimation model is constructed, and the fusion parameters are dynamically adjusted, including the following steps:

[0098] S31, in discrete time, define a 12-dimensional system vector:

[0099] ;

[0100] wherein, For three-dimensional position coordinates, For three-dimensional velocity components, For three-dimensional attitude angles, For roll angle, The pitch angle, Yaw angle These are the three-dimensional angular velocity components.

[0101] S32. For the uniform motion model, the position update part:

[0102] ;

[0103] in, It represents the time interval between two adjacent discrete time points.

[0104] Then the state transition matrix Represented as:

[0105] ;

[0106] Introduce a dynamic compensation term into the state transition matrix:

[0107] When the IMU detects an acceleration rate of change greater than the threshold 5 Automatically activate compensation coefficient , The value range is 1.2 to 1.8, and the position update part... Revised to .

[0108] S33. Construct based on the measurement characteristics of RTK and IMU 3D measurement matrix When RTK measures the location, It is the identity matrix, i.e. This represents the position coordinates portion of the state vector corresponding to the position information measured by RTK; when the IMU measures acceleration and angular velocity, it corresponds to the respective row vector.

[0109] Assuming the acceleration measured by the IMU is in The directional components are respectively , , angular velocity at The directional components are respectively , , Then the measurement matrix for:

[0110] ;

[0111] The first 3 rows correspond to the position measurement of RTK, mapping the position coordinate part in the state vector; the middle 3 rows correspond to the acceleration measurement of IMU, mapping the velocity component part in the state vector; and the last 3 rows correspond to the angular velocity measurement of IMU, mapping the angular velocity component part in the state vector.

[0112] Introducing an observation correction factor in the measurement matrix , the value range of which is 0.8 1.0:

[0113] The threshold of the preset signal-to-noise ratio is 35dB, the threshold of the height is 5000m, the satellite signal-to-noise ratio of RTK is monitored in real time , and when or h , the unit matrix of the first 3 rows is corrected to . S34, the dynamic adjustment of the fusion weight is specifically:

[0114] The fusion weight of RTK is defined as , the fusion weight of IMU is defined as

[0115] , and the calculation formula is: ;

[0116] ;

[0117] wherein, the satellite signal-to-noise ratio of RTK is

[0118] , the number of satellites received by RTK is , the IMU zero offset balance coefficient is , the initial value range is 0.8 1.2, the IMU zero offset estimation value is , the IMU error accumulation balance coefficient is , the value range is 0.03 0.08, and the error accumulation time of IMU is .

[0119] The output of the fusion model is:

[0120] ;

[0121] wherein, the denoised RTK positioning data is , and the corrected IMU fusion data is .

[0122] S35, the dynamic adjustment process noise covariance and measurement noise covariance .

[0123] If it is necessary to adapt to high dynamic scenes (such as rapid maneuvering of unmanned aerial vehicles), a compensation term of acceleration rate of change can be added process noise covariance The expression is as follows:

[0124] ;

[0125] wherein, is the initial process noise covariance, is a dynamic compensation coefficient, and the value is between 0.1 and 0.3.

[0126] measurement noise covariance The expression is as follows:

[0127] ;

[0128] wherein, is a residual influence coefficient, is Kalman filter innovation, is the 2-norm of information; The first block of the diagonal matrix is the RTK measurement noise, wherein, is the RTK measurement noise covariance; the second block is the IMU acceleration measurement noise, is the IMU acceleration measurement noise covariance, is a bias influence coefficient; the third block is the IMU angular velocity measurement noise is a time accumulation influence coefficient, is the IMU angular velocity measurement noise covariance.

[0129] S4, the preprocessed data is divided into a training set, a validation set and a test set according to a ratio of 7:2:1, the training set is used to train and optimize the fusion state estimation model, and the optimization target is to minimize the root mean square error (RMSE) of positioning error, and the expression is as follows:

[0130] ;

[0131] wherein, is the total number of training samples, indicates the index number of the training sample, and is used to identify the th training sample, is the model output value of the th training sample, is the true positioning value of the th training sample.

[0132] The application also adopts a hybrid optimization algorithm of particle swarm optimization (PSO) combined with local gradient descent, and optimizes model sensitive parameters, IMU zero offset balance coefficients , IMU error accumulation balance coefficients , filter order and cut-off frequency to solve the problems of traditional optimization methods, such as easy to fall into local optimum and slow convergence speed in navigation positioning scene.

[0133] To solve the problem of overfitting of single RMSE target in complex environment, the application constructs a composite fitness function which fuses positioning accuracy, stability and convergence speed :

[0134] ;

[0135] Among them, the minimum root mean square of positioning error is used to evaluate the stability; the standard deviation of positioning error is used to evaluate the stability and error fluctuation; is the filter convergence time of the model on the verification set, which is used to evaluate the dynamic performance; , , are weight coefficients, which are set to , , through prior experiments, and the composite function can guide the particle to search for the parameter solution which achieves the best balance among accuracy, stability and rapidity.

[0136] Dynamic adjustment of inertia weight, the inertia weight of the traditional PSO is a time linear function, and the application proposes an inertia weight based on dynamic adjustment of RTK signal quality :

[0137] ;

[0138] Among them, is the basic inertia weight, which adopts a linear decreasing strategy from 0.9 to 0.4; is the average satellite signal-to-noise ratio of the training data subset corresponding to the current particle; is the maximum possible value of signal-to-noise ratio; =0.2 is the adjustment coefficient.

[0139] The design increases the inertia weight to enhance the global exploration ability in the environment with poor signal (low SNR); in the environment with good signal, the weight is reduced to strengthen the local development, and the intelligent matching of search strategy and physical environment is realized.

[0140] Speed ​​constraints based on parameter sensitivity: To prevent over-searching on sensitive parameters, this invention uses parameter pairs... Historical sensitivity To constrain its maximum speed :

[0141] ;

[0142] in, =0.1 is the proportionality coefficient. It is to prevent small quantities from being divided by zero. The parameter to be optimized is the sensitive parameter, the IMU zero-bias balance coefficient. IMU error accumulation balance coefficient Filter order or cutoff frequency One of them; this constraint makes sensitive parameters such as the IMU zero-bias balance coefficients... By performing fine-grained searches and fast searches for insensitive parameters such as 'n', the overall search efficiency is improved.

[0143] Physics Model-Guided Local Optimization Phase: Initial Point Setting: Globally Optimal Parameters Output by Environment Adaptive PSO As an initial point, the gradient guided by the IMU error model is calculated: abandoning traditional numerical differentiation, this invention utilizes the physical model of the IMU error to construct a more guiding gradient direction. For example, its gradient direction is dominated by the integral term of the IMU velocity error:

[0144] ;

[0145] in, It is the corrected IMU acceleration. It is a zero-biased estimate. The optimized IMU zero-bias balance coefficients. The optimal IMU error accumulation balance coefficient is the result of optimization. This invention is based on gradient calculation with physical meaning, which is more stable and conforms to the inherent laws of sensor fusion than pure mathematical numerical differentiation, and can converge to the physically reasonable extreme point more quickly.

[0146] For mixed parameter spaces ( n For discrete parameters, the rest are continuous, and a grouped alternating optimization strategy is adopted: for fixed discrete parameters n The gradient descent method of the above physical model is used to optimize the continuous parameters. and For fixed continuous parameters, an exhaustive method is used in... n Evaluate and select the optimal one within the integer range of [2,6]. n .

[0147] Repeat the above step 3 times, or until the RMSE changes less than This strategy effectively solves the problem of mixed variable optimization and ensures the optimality of discrete parameters.

[0148] Each link of optimization, such as composite fitness function, environment adaptive weight, and parameter sensitivity constraint, is closely designed around the actual problems of RTK / IMU fusion in complex environment, such as precision, stability, and convergence speed balance.

[0149] S5, evaluate the core performance indicators of the model through the validation set, the core performance indicators of the model include positioning accuracy, response time and error suppression ability, the positioning accuracy includes plane positioning error and height positioning error, the response time includes weight adjustment response delay and filter convergence time, and the error suppression ability includes long-time signal interruption error and environment noise suppression rate.

[0150] The plane positioning error CEP95 is defined as 95% of the positioning points falling within a circular area with the true position as the center, and the radius of the circle is CEP95, which reflects the positioning accuracy of the model in the horizontal direction; the height positioning error LE95 is defined as the absolute value of the error of 95% of the positioning points in the height direction is less than LE95, which reflects the accuracy of the model in the vertical direction.

[0151] The adjustment response delay is defined as the time from the sudden change of RTK signal quality or IMU noise to the stable fusion weight, which reflects the rapid adaptation ability of the model to environmental changes; the filter convergence time is defined as the time when the position item variance of the state covariance matrix of the model is reduced to 1x10 -5 m 2 after the model starts or the signal interruption is restored, which reflects the rapid stability of the model.

[0152] The long-time signal interruption error is defined as the cumulative amount of positioning error at different time lengths after the RTK signal is completely interrupted, which reflects the effectiveness of the IMU error suppression algorithm; the environment noise suppression rate is defined as the ratio of the noise variance of the model output data to the noise variance of the original sensor data, which reflects the suppression ability of the model to environmental noise.

[0153] S6, verify the stability of the model in actual scenes using the test set.

[0154] Embodiment 1, the effectiveness of the present application is illustrated by the application of a certain Danxia landform inspection positioning:

[0155] I. Implementation scenario:

[0156] Location: the core area of a Danxia landform scenic spot, 1500m above sea level, with an average annual rainfall of 320mm, belonging to a typical arid red layer landform, with drought-resistant sand sagebrush shrubs in some areas, rock pillars and gullies spaced 200-600m apart, the inspection unmanned aerial vehicle flying at a height of 80m and a speed of 4m / s.

[0157] Requirement: The unmanned aerial vehicle needs to conduct inspection on a Danxia landform, with a positioning deviation of ≤0.3m, and when encountering cliff blockage, gully blockage and shrub blockage leading to RTK signal interruption, it needs to achieve continuous positioning for ≥5 minutes to ensure the continuity of surveying and mapping data and hidden danger point positioning.

[0158] II. Equipment used

[0159] The RTK equipment uses Huichu T7 GNSS receiver, supports Beidou and GPS dual mode, and has a positioning accuracy of 1cm+1ppm; the IMU sensor uses ADI ADIS16488, which has a zero offset stability of ≤0.1° / h, a range of ±10g / ±200° / s, and can accurately capture the attitude changes of the unmanned aerial vehicle flying in the Danxia broken terrain; the data processing terminal uses NVIDIA Jetson Xavier NX, with a computing power of 21TOPS, supporting real-time algorithm operation, meeting the needs of Danxia landform field mobile mapping and hidden danger investigation.

[0160] III. Implementation steps

[0161] ① Data acquisition and preprocessing

[0162] Synchronous acquisition of RTK raw position data , IMU three-axis acceleration =0.3-2.0g suitable for the rapid acceleration changes of Danxia cliff climbing / gully descending, angular velocity =0.8-4.0° / s, timestamp synchronization error controlled within 8ms to ensure data timing consistency.

[0163] The Gaussian filter window is set to 8 frames, which is suitable for the RTK signal fluctuations caused by Danxia landform vegetation blockage and cliff reflection, and the RTK position jump amplitude is reduced from 0.6m to 0.12m after denoising.

[0164] Static calibration, simulate local 1500m altitude low pressure to obtain zero offset: =0.02g, =0.018g, =0.022g, =0.049° / h, =0.044° / h, =0.054° / h, the corrected IMU data error is reduced by 43%; the Butterworth filter =8Hz, =4 order, filter out the high-frequency vibration of the UAV body above 15 Hz caused by the broken cliff of Danxia, and retain the effective inertial measurement signal.

[0165] ② Fusion model construction and parameter optimization:

[0166] State transition matrix =0.08s, adapt to the flight speed of the UAV, set the acceleration change rate threshold to 4.2m / s 3 When the UAV climbs steep slopes =1.65, correction =0.129s, improve the state prediction accuracy in steep slope and canyon transition scenarios.

[0167] Measurement matrix : When the RTK satellite signal-to-noise ratio SNR=30dB is blocked by some Danxia sand jujube shrubs, at this time =0.87; the current altitude is 1500m <5000m elevation threshold, =1.0, balance the measurement weight under different signal quality through observation correction factor.

[0168] Fusion weight:

[0169] In open areas (i.e. without obstruction) SNR=34dB, N=7 satellites:

[0170] , , prefer to trust RTK data.

[0171] When N=2 satellites are blocked by cliffs and gullies:

[0172] , , dynamically improve the IMU weight to ensure continuous positioning.

[0173] Noise covariance: Position item variance 1e-4m 2 , =0.629, ; =1.7e-3m 2 Adapt to the RTK measurement noise in vegetation / cliff blocking scenarios, =1.25e-2 (m / s 2 ) 2 , =1.45e-3 (rad / s) 2 , accurately match the noise characteristics of Danxia topography.

[0174] ③ Model training and performance verification:

[0175] The historical inspection data of 140,000 frames of the Danxia landform, 40,000 frames of the verification set and 20,000 frames of the test set are selected, and the optimized parameters are as follows: = 0.22, = 0.11, = 9 Hz, = 4 orders, and the RMSE after training is 0.13 m.

[0176] Test results: the plane CEP95 is 0.25 m, and the elevation LE95 is 0.33 m; when the RTK signal is interrupted for 300 s due to the shielding of cliff walls and gullies, the positioning error is 0.92 m; the weight adjustment response delay is 0.09 s, which fully meets the core requirements of the Danxia landform in terms of high precision, anti-shielding and stable continuity for surveying and mapping and hidden danger investigation.

[0177] Four, implementation effect:

[0178] As shown in Figure 3 , the method is applied in a Danxia landform scenic area for multiple times, and the Danxia core area is surveyed and mapped, and the rock paintings and geological disaster hidden danger points are marked, and no problems such as missing marking, wrong marking or terrain mapping distortion and hidden danger point misjudgment caused by positioning deviation occur; compared with the traditional fixed weight fusion method (CEP95=0.83 m), the coincidence degree of the surveying and mapping trajectory is increased from 96.5% to 99.7%.

[0179] The single-region surveying and mapping time is shortened from 125 minutes to 105 minutes, and the efficiency and accuracy of the Danxia landform surveying and mapping and geological hidden danger investigation are greatly improved, which provides reliable positioning data support for subsequent scenic area protection planning, rock painting repair and geological disaster prevention.

[0180] Therefore, the RTK and IMU fusion state estimation method based on particle swarm optimization is adopted, which solves the core pain points of the existing RTK and IMU fusion technology in complex environments, especially in mountainous and high-altitude areas, and can realize high-precision, fast-response and strong-stable positioning, which provides reliable technical support for mountain surveying and mapping, wind power inspection, highway construction and other scenes, and has low cost, easy promotion and engineering advantages, and has significant practical value and market prospect.

[0181] Finally, it should be pointed out that: the above examples are only used to illustrate the technical solutions of the present application but not to limit it, although the present application has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present application can be modified or equivalently replaced, and these modifications or equivalent replacements cannot make the modified technical solutions deviate from the spirit and scope of the technical solutions of the present application.

Claims

1. A state estimation method based on particle swarm optimization for fusion of RTK and IMU, characterized in that, Includes the following steps: S1. Synchronously collect raw data under different environments using RTK equipment and IMU sensors; S2. Preprocess the acquired raw data, including denoising the RTK location data and correcting and filtering the IMU data; S3. Construct a fusion state estimation model based on the adaptive Kalman filter algorithm and dynamically adjust the fusion parameters; S4. A hybrid optimization algorithm combining particle swarm optimization and local gradient descent is used to optimize the parameters of the self-fusion state estimation model. S5. Evaluate the core performance metrics of the model using the validation set; S6. Verify model stability in real-world scenarios using a test set; In S2, the preprocessing of the collected raw data specifically involves: S21. A Gaussian filtering algorithm is used to denoise the raw location data collected by the RTK device. The calculation formula is as follows: ； in, Raw location data collected by RTK devices. For dynamic standard deviation, The mean of the data. This is the location data after noise reduction; S22. Obtain accelerometer zero bias through static calibration. and gyroscope zero bias First, perform zero-bias correction on the raw triaxial acceleration and triaxial angular velocity data acquired by the IMU sensor. The formula is as follows: ， ， ； ， ， ； in, , , For the corrected acceleration, , , The corrected angular velocity; Then, a Butterworth low-pass filter is used to remove high-frequency noise. Its transfer function is: in, For complex variables, The cutoff frequency, The order of the filter; In S4, a hybrid optimization algorithm combining Particle Swarm Optimization (PSO) and Local Gradient Descent is employed, targeting model-sensitive parameters including the IMU zero-bias balance coefficient. IMU error accumulation balance coefficient Filter order and cutoff frequency Optimization is performed; in the particle swarm optimization phase, the optimal parameter range is quickly located through group collaboration, as follows: Constructing a composite fitness function : ； in, To minimize the root mean square of the positioning error; This represents the standard deviation of the positioning error; It is the filtering convergence time of the model on the validation set; , , These are the weighting coefficients; Dynamically adjust inertia weight The formula is as follows: ； in, It is the basic inertia weight; It is the average satellite signal-to-noise ratio of the subset of training data corresponding to the current particle; It is the maximum possible value of the signal-to-noise ratio; It is the adjustment coefficient; Based on parameters Historical sensitivity To constrain its maximum speed : ； in, This is the proportionality coefficient. It is to prevent small quantities from being divided by zero. These are the parameters to be optimized. Global optimal parameters output by environment-adaptive PSO As the initial point, then The gradient direction is dominated by the integral term of the IMU velocity error: ； in, It is the corrected IMU acceleration. It is a zero-biased estimate. The optimized IMU zero-bias balance coefficients. This is the optimal IMU error accumulation balance coefficient after optimization.

2. The state estimation method based on particle swarm optimization for RTK and IMU fusion according to claim 1, characterized in that, In S1, satellite positioning data and corresponding timestamps are collected using RTK equipment. Satellite positioning data includes longitude ,latitude and height raw location data Simultaneously, raw triaxial acceleration is acquired via an IMU sensor. and the original triaxial angular velocity data And record the corresponding timestamp. .

3. The state estimation method for RTK and IMU fusion based on particle swarm optimization according to claim 2, characterized in that, In S3, a fusion state estimation model is constructed based on the adaptive Kalman filter algorithm, specifically including the following steps: S31. Define the system vector in discrete time: ； in, For three-dimensional position coordinates, For three-dimensional velocity components, For three-dimensional attitude angles, For roll angle, The pitch angle, Yaw angle These are three-dimensional angular velocity components; S32. Construct the state transition matrix The expression is as follows: ； in, This represents the time interval between two adjacent discrete time points; and a dynamic compensation term is introduced into the state transition matrix. S33. Construct a measurement matrix based on the measurement characteristics of RTK and IMU. The expression is as follows: ； The first three rows correspond to the RTK position measurement, mapping the position coordinates to the state vector; the middle three rows correspond to the IMU acceleration measurement, mapping the velocity components to the state vector; and the last three rows correspond to the IMU angular velocity measurement, mapping the angular velocity components to the state vector. An observation correction factor based on satellite signal-to-noise ratio and elevation is introduced into the measurement matrix. ; S34. Dynamically adjust the fusion weights, defining the RTK fusion weights as follows: The fusion weight of the IMU is The calculation formula is: ； ； in, For RTK satellite signal-to-noise ratio, The number of satellites received by RTK. This is the IMU zero-bias balance coefficient. This is the zero-bias estimate of the IMU. This is the IMU error accumulation balance factor. This is the error accumulation time for the IMU; The output of the fusion model is: ； in, This is the denoised RTK positioning data. This is the corrected IMU fused data; S35, Dynamic Adjustment Process Noise Covariance and measurement noise covariance : Add compensation term for rate of change of acceleration Process noise covariance The expression is as follows: ； in, The initial process noise covariance, For dynamic compensation coefficients; Measurement noise covariance The expression is as follows: ； in, This is the residual influence coefficient. For Kalman filtering information, The 2-norm of the information; The first block of the diagonal matrix is ​​the RTK measurement noise, where, The first block represents the noise covariance for RTK measurements; the second block represents the noise from IMU acceleration measurements. For IMU acceleration measurement noise covariance, The zero-bias influence coefficient; the third block represents the IMU angular velocity measurement noise. This is the cumulative impact coefficient over time. Noise covariance for IMU angular velocity measurements.

4. The state estimation method for RTK and IMU fusion based on particle swarm optimization according to claim 3, characterized in that, In S4, the preprocessed data is randomly divided into training, validation, and test sets in a 7:2:1 ratio. The training set is used to iteratively optimize the model parameters, with the optimization objective being to minimize the root mean square of the localization error. : ； in, The total number of training samples, This represents the index number of the training sample, used to identify the first... training samples, For the first The model output value for each training sample. For the first The true localization value of each training sample.

5. The state estimation method for RTK and IMU fusion based on particle swarm optimization according to claim 1, characterized in that, In S5, the core performance indicators of the model include positioning accuracy, response time, and error suppression capability. Positioning accuracy includes planar positioning error and elevation positioning error. Response time includes weight adjustment response delay and filtering convergence time. Error suppression capability includes long-term signal interruption error and environmental noise suppression rate.

6. The state estimation method for RTK and IMU fusion based on particle swarm optimization according to claim 5, characterized in that, In S5, the planar positioning error is defined as 95% of the positioning points falling within a circular area centered on the actual position, reflecting the model's positioning accuracy in the horizontal direction. Elevation positioning error is defined as the absolute value of the error in the elevation direction of 95% of the positioning points being less than LE95, reflecting the accuracy of the model in the vertical direction.

7. The state estimation method for RTK and IMU fusion based on particle swarm optimization according to claim 5, characterized in that, In S5, the adjustment response delay is defined as the time from a sudden change in RTK signal quality or IMU noise to the fusion weights stabilizing at the new target value, reflecting the model's ability to quickly adapt to environmental changes. The filter convergence time is defined as the state covariance matrix after model startup or signal interruption recovery. The variance of the position term was reduced to 1×10 -5 m 2 The time required below reflects the model's ability to stabilize quickly.

8. The state estimation method for RTK and IMU fusion based on particle swarm optimization according to claim 5, characterized in that, In S5, the long-term signal interruption error is defined as the cumulative amount of positioning error for different durations after the RTK signal is completely interrupted, reflecting the effectiveness of the IMU error suppression algorithm; The environmental noise suppression rate is defined as the ratio of the noise variance of the model output data to the noise variance of the original sensor data, reflecting the model's ability to suppress environmental noise.

Images