Weighted fusion agricultural robot positioning method and system based on deviation filtering

Abstract

The invention relates to the technical field of data processing and robot positioning, and specifically to a weighted fusion agricultural robot positioning method and system based on deviation filtering, which obtains state data of a real-time dynamic differential positioning system and an inertial measurement unit, obtains a priori estimate of the state at the current moment, inputs a dynamic model, obtains an observation value, and then introduces a deviation mean based on the difference between the observation value and the actual measurement value, corrects the priori estimate of the state based on the deviation mean, obtains a posterior estimate of the state, introduces a smoothing factor based on the difference between the posterior estimate of the state of the real-time dynamic differential positioning system and the inertial measurement unit, and obtains the weight factor at the current moment, and performs adaptive weighted fusion processing, so as to realize high-precision positioning of the agricultural robot and improve the robustness and anti-interference ability of the system.

Description

Technical Field

[0001] The present invention relates to the technical fields of data processing and robot positioning, and in particular to a weighted fusion agricultural robot positioning method and system based on deviation filtering, with classification number: G01C21 / 28. Background Art

[0002] With the development of modern agriculture, agricultural robots are increasingly used in farmland management. When these robots are working in the field, they need a high-precision and high-stability positioning system to ensure the accuracy of their navigation and operation. At present, commonly used positioning technologies include real-time dynamic differential positioning system (RTK) and inertial measurement unit (IMU). RTK can provide high-precision geographic location information by receiving satellite signals, and its positioning error will not accumulate over time, but the RTK system is heavily dependent on satellite signals. When agricultural robots operate under the shade of trees, next to buildings or other shielded environments, RTK signals are easily interfered or interrupted, resulting in reduced positioning accuracy; while IMU can provide the robot's posture and motion information by measuring acceleration and angular velocity, and its anti-interference ability is strong, but due to the inherent noise and drift of the sensor, long-term use will lead to cumulative errors, causing the positioning information to gradually distort. Therefore, it is difficult to simultaneously meet high-precision positioning and anti-interference requirements by relying solely on RTK or IMU.

[0003] At present, there are also some studies and applications that try to combine RTK and IMU in order to make up for their respective shortcomings. However, the existing fusion methods still have many problems in practical applications. For example, the high complexity and large computational load make it difficult to run efficiently on agricultural robots with limited resources. The performance in dynamic environments is still not ideal, especially when the satellite signal is suddenly interrupted or severely interfered, the positioning strategy cannot be adjusted in time, resulting in the robot being unable to work stably. In agricultural operations with high real-time requirements, the response speed and robustness of the existing fusion methods are insufficient, and it is difficult to cope with sudden environmental changes and signal fluctuations. Summary of the invention

[0004] The present invention provides a weighted fusion agricultural robot positioning method and system based on deviation filtering.

[0005] The technical solution of the present invention is as follows:

[0006] A weighted fusion agricultural robot positioning method based on deviation filtering includes the following steps:

[0007] S1. Based on the real-time dynamic differential positioning system and the inertial measurement unit, the actual measurement values ​​of the position and motion posture of the agricultural robot at different times are respectively obtained, and the real-time dynamic differential positioning state data and the inertial measurement state data are correspondingly obtained;

[0008] S2, inputting the acquired real-time dynamic differential positioning state data and inertial measurement state data into the dynamics model, and obtaining a priori estimation of the real-time dynamic differential positioning state and a priori estimation of the inertial measurement state at the current moment respectively;

[0009] Based on the state covariance matrix of the previous moment, the Jacobian matrix of the state transfer matrix and the covariance matrix of the process noise, the corresponding prediction covariance matrix of the current moment is obtained;

[0010] S3, inputting the real-time dynamic differential positioning state prior estimation and the inertial measurement state prior estimation into the dynamic model respectively, and obtaining the observation values ​​respectively; calculating the difference between the observation value and the actual measurement value, using a sliding window to traverse the historical moments, and averaging the obtained several differences, and obtaining the deviation mean of the real-time dynamic differential positioning system and the inertial measurement unit respectively;

[0011] Based on the prediction covariance matrix, the Jacobian matrix of the state observation matrix and the observation noise covariance matrix, the Kalman gains of the real-time dynamic differential positioning system and the inertial measurement unit are obtained respectively;

[0012] Based on the difference between the observed value and the actual measured value, the deviation mean and the Kalman gain, the state prior estimation is corrected to obtain the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit respectively;

[0013] Based on the Kalman gain and Jacobian matrix of the state observation matrix, the prediction covariance matrix is ​​adjusted to obtain the posterior covariance matrix of the real-time dynamic differential positioning system and the inertial measurement unit respectively;

[0014] S4, based on the difference of the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit, a smoothing factor is introduced to adaptively adjust the weight factor of the previous moment, and the weight factors of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment are obtained respectively, and the weight factors are normalized to obtain the fusion weight;

[0015] Based on the fusion weight, the state posterior estimates of the real-time dynamic differential positioning system and the inertial measurement unit are adaptively weighted and fused to obtain the fused state estimate; the posterior covariance matrix is ​​adaptively weighted and fused to obtain the fusion covariance, and the agricultural robot is positioned based on the fusion state estimate and the fusion covariance.

[0016] Specifically, based on the difference of the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit, a smoothing factor is introduced to adaptively adjust the weight factor of the previous moment, and the weight factors of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment are obtained respectively. The steps are as follows:

[0017] If the difference between the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment is greater than the set adjustment threshold, the smoothing factor at the previous moment is subtracted from the specific adjustment variable;

[0018] Otherwise, add the previous moment smoothing factor to the specific adjustment variable to get the current moment smoothing factor;

[0019] Based on the current moment smoothing factor and the difference between the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment, the weight factor of the previous moment is adaptively adjusted to obtain the weight factors of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment. , , the formula is:

[0020] ,

[0021] in, is the adjustment threshold of the smoothing factor, is the current moment smoothing factor, is the smoothing factor at the previous moment, β is the adjustment variable;

[0022] ,

[0023] in, is the inertial measurement unit at the last moment, k -1 moment weight factor, It is the real-time dynamic differential positioning system at the last moment. k -1 moment weight factor, is the current moment smoothing factor, is the a posteriori estimate of the state of the inertial measurement unit, It is the state posterior estimate of the real-time dynamic differential positioning system.

[0024] Specifically, in S3, the real-time dynamic differential positioning state prior estimation and the inertial measurement state prior estimation are respectively input into the dynamic model to obtain observation values ​​respectively; the difference between the observation value and the actual measurement value is calculated, and the historical moments are traversed using a sliding window to obtain a number of differences and then average them to obtain the deviation mean of the real-time dynamic differential positioning system and the inertial measurement unit respectively.

[0025] For an inertial measurement unit, the formula is expressed as:

[0026] ,

[0027] in, is the observation value of the inertial measurement unit, is the observation equation, is the prior estimate of the inertial measurement state;

[0028] ,

[0029] in, is the error value, that is, the difference between the observed value and the actual measured value, is the actual measured value, is the observed value;

[0030] ,

[0031] in, is the mean bias of the inertial measurement unit, N is the size of the sliding window, i is the index of each data point in the sliding window, k is the present moment, is the difference between the observed value and the actual measured value;

[0032] For the real-time dynamic differential positioning system, the formula is expressed as:

[0033] ,

[0034] in, is the observation value of the real-time dynamic differential positioning system, is the observation equation, is the prior estimate of the inertial measurement state;

[0035] ,

[0036] in, is the error value, that is, the difference between the observed value and the actual measured value, is the actual measured value, is the observed value;

[0037] ,

[0038] in, is the mean deviation of the real-time dynamic differential positioning system, N is the size of the sliding window, i is the index of each data point in the sliding window, k is the present moment, is the difference between the observed value and the actual measured value.

[0039] Furthermore, based on the difference between the observed value and the actual measured value, the deviation mean and the Kalman gain, the state prior estimation is corrected to obtain the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit, which is specifically expressed as follows:

[0040] For the Inertial Measurement Unit:

[0041] ,

[0042] in, is the a posteriori estimate of the state of the inertial measurement unit, is the prior estimate of the inertial measurement state, is the Kalman gain, is the actual measured value, is the observed value, is the mean of the deviations;

[0043] For real-time dynamic differential positioning system:

[0044] ,

[0045] in, is the state posterior estimate of the real-time dynamic differential positioning system, is the prior estimate of the real-time dynamic differential positioning state, is the Kalman gain, is the actual measured value, is the observed value, is the mean deviation.

[0046] Specifically, in S4, the weight factors of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment are normalized to obtain a fusion weight, which is expressed as follows:

[0047] ,

[0048] in, is the fusion weight of the inertial measurement unit, is the fusion weight of the real-time dynamic differential positioning system, is the weight factor of the real-time dynamic differential positioning system, is the weight factor of the inertial measurement unit;

[0049] Based on the fusion weight, the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit is adaptively weighted and fused to obtain the fusion state estimation. , specifically:

[0050] ,

[0051] in, is the fusion weight of the inertial measurement unit, is the fusion weight of the real-time dynamic differential positioning system, is the a posteriori estimate of the state of the inertial measurement unit, It is the state posterior estimate of the real-time dynamic differential positioning system;

[0052] The posterior covariance matrix of the real-time dynamic differential positioning system and the inertial measurement unit is adaptively weighted and fused to obtain the fused covariance , specifically:

[0053] ,

[0054] in, is the a posteriori covariance matrix of the inertial measurement unit, is the posterior covariance matrix of the real-time dynamic differential positioning system.

[0055] Furthermore, in S2, based on the acquired real-time dynamic differential positioning state data and inertial measurement state data, the acquired state data is input into the dynamic model to obtain a priori estimate of the real-time dynamic differential positioning state and a priori estimate of the inertial measurement state at the current moment, specifically:

[0056] The acquired real-time dynamic differential positioning state data and inertial measurement state data are input into the dynamic model to obtain the state estimation data of the real-time dynamic differential positioning system and the inertial measurement unit at the previous moment. Based on the state estimation data at the previous moment, the dynamic model is input to obtain the prior estimation of the real-time dynamic differential positioning state and the prior estimation of the inertial measurement state at the current moment. The formula is expressed as follows:

[0057] For the Inertial Measurement Unit:

[0058] ,

[0059] in, is the state estimation data of the inertial measurement unit at the previous moment, i.e., moment k-1, is the control input at the previous moment, i.e., moment k-1; To obtain the current state estimation data based on the k-1 moment, that is, the inertial measurement state prior estimate;

[0060] For real-time dynamic differential positioning system:

[0061] ,

[0062] in, It is to obtain the prior estimate of the real-time dynamic differential positioning state at the current moment based on the k-1 moment. is the state estimation data of the real-time dynamic differential positioning system at time k-1, is the control input at time k-1.

[0063] Furthermore, the prediction covariance matrix at the current moment is obtained based on the state covariance matrix at the previous moment, the Jacobian matrix of the state transfer matrix, and the covariance matrix of the process noise in S2, and the formula is expressed as follows:

[0064] For the Inertial Measurement Unit:

[0065] ,

[0066] in, is the predicted covariance matrix of the inertial measurement unit at the current moment, is the covariance matrix of the inertial measurement unit at the previous moment, i.e., moment k-1, is the Jacobian matrix of the state transfer matrix of the inertial measurement unit at time k-1, is the covariance matrix of the process noise at time k-1;

[0067] For real-time dynamic differential positioning system:

[0068] ,

[0069] in, is the prediction covariance matrix of the real-time dynamic differential positioning system at the current moment, is the covariance matrix of the real-time dynamic differential positioning system at the previous moment, i.e., moment k-1, is the Jacobian matrix of the state transfer matrix of the real-time dynamic differential positioning system at time k-1, is the covariance matrix of the process noise at time k-1.

[0070] Specifically, in S3, the prediction covariance matrix is ​​adjusted based on the Kalman gain and the Jacobian matrix of the state observation matrix to obtain the posterior covariance matrix of the real-time dynamic differential positioning system and the inertial measurement unit, which is specifically expressed as follows:

[0071] For the Inertial Measurement Unit:

[0072] ,

[0073] in, is the a posteriori covariance matrix of the inertial measurement unit, I is the identity matrix, is the Kalman gain, is the Jacobian matrix of the state observation matrix, is the predicted covariance matrix of the inertial measurement unit;

[0074] For real-time dynamic differential positioning system:

[0075] ,

[0076] in, is the posterior covariance matrix of the real-time dynamic differential positioning system, I is the identity matrix, is the Kalman gain, is the Jacobian matrix of the state observation matrix, is the prediction covariance matrix of the real-time dynamic differential positioning system.

[0077] Furthermore, the dynamic model of the real-time dynamic differential positioning system and the inertial measurement unit is expressed as follows:

[0078] ,

[0079] ,

[0080] in, yes k +1 status data at the moment, yes k The state vector of the real-time dynamic differential positioning system or inertial measurement unit, i.e., the state data; yes k The control input at the moment, is the state equation for state data and control input, yes k The process noise at each moment; yes k +1 measured value at time, yes k About status data at all times The observation equation is yes k The measurement noise at the moment.

[0081] The present invention also provides a weighted fusion agricultural robot positioning system based on deviation filtering, comprising:

[0082] Data acquisition module: used to obtain the actual measurement values ​​of the position and motion posture of the agricultural robot at different times based on the real-time dynamic differential positioning system and the inertial measurement unit, and obtain the real-time dynamic differential positioning state data and the inertial measurement state data accordingly;

[0083] State prediction module: input the acquired real-time dynamic differential positioning state data and inertial measurement state data into the dynamic model to obtain the real-time dynamic differential positioning state prior estimation and inertial measurement state prior estimation at the current moment respectively; based on the state covariance matrix at the previous moment, the Jacobian matrix of the state transfer matrix and the covariance matrix of the process noise, obtain the corresponding prediction covariance matrix at the current moment;

[0084] State update module: used to input the real-time dynamic differential positioning state prior estimate and the inertial measurement state prior estimate into the dynamic model respectively, and obtain the observation value respectively; calculate the difference between the observation value and the actual measurement value, use the sliding window to traverse the historical moments, obtain several differences and then average them, and obtain the deviation mean of the real-time dynamic differential positioning system and the inertial measurement unit respectively; based on the prediction covariance matrix, the Jacobian matrix of the state observation matrix and the observation noise covariance matrix, the Kalman gain of the real-time dynamic differential positioning system and the inertial measurement unit are obtained respectively; based on the difference between the observation value and the actual measurement value, the deviation mean and the Kalman gain, the state prior estimate is corrected to obtain the state posterior estimate of the real-time dynamic differential positioning system and the inertial measurement unit respectively; based on the Kalman gain and the Jacobian matrix of the state observation matrix, the prediction covariance matrix is ​​adjusted to obtain the posterior covariance matrix of the real-time dynamic differential positioning system and the inertial measurement unit respectively;

[0085] Adaptive weighted fusion module: It is used to introduce a smoothing factor to adaptively adjust the weight factor of the previous moment based on the difference of the posterior state estimate of the real-time dynamic differential positioning system and the inertial measurement unit, and obtain the weight factors of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment, respectively, and normalize them to obtain the fusion weight; based on the fusion weight, the posterior state estimate of the real-time dynamic differential positioning system and the inertial measurement unit is adaptively weighted and fused to obtain the fusion state estimate; the posterior covariance matrix is ​​adaptively weighted and fused to obtain the fusion covariance, and the agricultural robot is positioned based on the fusion state estimate and the fusion covariance.

[0086] The beneficial effects of the present invention are:

[0087] 1. The present invention introduces a deviation mean based on the difference between the observed value and the actual measured value, and processes the IMU and RTK data respectively based on the introduced deviation mean to make them smoother and suppress abnormal data fluctuations as much as possible, reduce the impact of disturbances, and then perform adaptive weighted fusion processing on the processed IMU and RTK data, which can improve the stability and accuracy of the positioning data based on the original data, thereby ensuring that the agricultural robot can operate continuously and stably.

[0088] 2. The weighted fusion agricultural robot positioning system based on deviation filtering provided by the present invention has an adaptive adjustment function. When the RTK satellite signal is interfered or interrupted, the fusion weight can be adjusted to reduce its weight and switch to the IMU-dominated positioning solution to ensure the normal operation of the robot. This dynamic adjustment mechanism enables the robot to maintain high-precision positioning in various complex and harsh environments, reducing the risk of operation interruption due to signal fluctuations; even when the signal is blocked, the robot can still rely on IMU data for efficient and accurate positioning to ensure operation continuity and reliability.

[0089] 3. The present invention combines RTK with IMU to obtain motion state data about the agricultural robot with better reliability, higher accuracy and faster data update rate. By processing the motion state data, not only can high-precision positioning of the agricultural robot be achieved, but also the overall cost of the sensor and the system can be reduced, which is suitable for large-scale agricultural robot deployment. By fusing multi-source sensor data, the present invention significantly improves the robustness and anti-interference ability of the system. DETAILED DESCRIPTION

[0090] Example

[0091] This embodiment provides a weighted fusion agricultural robot positioning method based on deviation filtering, comprising the following steps:

[0092] S1. Based on the real-time dynamic differential positioning system and the inertial measurement unit, the actual measurement values ​​of the position and motion posture of the agricultural robot at different times are obtained respectively, and the real-time dynamic differential positioning state data and the inertial measurement state data are obtained accordingly.

[0093] S2, inputting the acquired real-time dynamic differential positioning state data and inertial measurement state data into the dynamics model, and obtaining a priori estimation of the real-time dynamic differential positioning state and a priori estimation of the inertial measurement state at the current moment respectively;

[0094] Based on the state covariance matrix of the previous moment, the Jacobian matrix of the state transfer matrix and the covariance matrix of the process noise, the corresponding prediction covariance matrix of the current moment is obtained.

[0095] The dynamic model of the real-time dynamic differential positioning system and the inertial measurement unit used in the present invention is expressed as follows:

[0096] ,

[0097] ,

[0098] in, yes k +1 status data at the moment, yes kThe state vector of the real-time dynamic differential positioning system or inertial measurement unit, i.e., the state data; yes k The control input at the moment, is the state equation for state data and control input, yes k The process noise at each moment; yes k +1 measured value at time, yes k About status data at all times The observation equation is yes k The measurement noise at the moment.

[0099] Based on the acquired real-time dynamic differential positioning state data and inertial measurement state data, the acquired state data is input into the dynamic model to obtain the current real-time dynamic differential positioning state prior estimation and inertial measurement state prior estimation, specifically:

[0100] The acquired real-time dynamic differential positioning state data and inertial measurement state data are input into the dynamic model to obtain the state estimation data of the real-time dynamic differential positioning system and the inertial measurement unit at the previous moment. Based on the state estimation data at the previous moment, the dynamic model is input to obtain the prior estimation of the real-time dynamic differential positioning state and the prior estimation of the inertial measurement state at the current moment. The formula is expressed as:

[0101] For the Inertial Measurement Unit:

[0102] ,

[0103] in, is the state estimation data of the inertial measurement unit at the previous moment, i.e., moment k-1, is the control input at the previous moment, i.e., moment k-1; To obtain the state estimation data of the current moment based on the k-1 moment, that is, the prior estimate of the inertial measurement state.

[0104] For real-time dynamic differential positioning system:

[0105] ,

[0106] in, It is to obtain the prior estimate of the real-time dynamic differential positioning state at the current moment based on the k-1 moment. is the state estimation data of the real-time dynamic differential positioning system at time k-1, is the control input at time k-1.

[0107] Based on the state covariance matrix of the previous moment, the Jacobian matrix of the state transfer matrix and the covariance matrix of the process noise, the corresponding prediction covariance matrix of the current moment is obtained, and the formula is expressed as:

[0108] For the Inertial Measurement Unit:

[0109] ,

[0110] in, is the predicted covariance matrix of the inertial measurement unit at the current moment, is the covariance matrix of the inertial measurement unit at the previous moment, i.e., moment k-1, is the Jacobian matrix of the state transfer matrix of the inertial measurement unit at time k-1, is the covariance matrix of the process noise at time k-1.

[0111] For real-time dynamic differential positioning system:

[0112] ,

[0113] in, is the prediction covariance matrix of the real-time dynamic differential positioning system at the current moment, is the covariance matrix of the real-time dynamic differential positioning system at the previous moment, i.e., moment k-1, is the Jacobian matrix of the state transfer matrix of the real-time dynamic differential positioning system at time k-1, is the covariance matrix of the process noise at time k-1.

[0114] S3, inputting the real-time dynamic differential positioning state prior estimation and the inertial measurement state prior estimation into the dynamic model respectively, and obtaining the observation values ​​respectively; calculating the difference between the observation value and the actual measurement value, using a sliding window to traverse the historical moments, and averaging the obtained several differences, and obtaining the deviation mean of the real-time dynamic differential positioning system and the inertial measurement unit respectively;

[0115] Based on the prediction covariance matrix, the Jacobian matrix of the state observation matrix and the observation noise covariance matrix, the Kalman gains of the real-time dynamic differential positioning system and the inertial measurement unit are obtained respectively;

[0116] Based on the difference between the observed value and the actual measured value, the deviation mean and the Kalman gain, the state prior estimation is corrected to obtain the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit respectively;

[0117] Based on the Kalman gain and the Jacobian matrix of the state observation matrix, the prediction covariance matrix is ​​adjusted to obtain the posterior covariance matrices of the real-time dynamic differential positioning system and the inertial measurement unit, respectively.

[0118] Specifically, the real-time dynamic differential positioning state prior estimation and the inertial measurement state prior estimation are input into the dynamic model to obtain observation values ​​respectively. Further, based on the difference between the observation value and the actual measurement value, the historical moments are traversed using a sliding window to obtain several differences and then average them to obtain the deviation mean of the real-time dynamic differential positioning system and the inertial measurement unit respectively.

[0119] For an inertial measurement unit, the formula is expressed as:

[0120] ,

[0121] in, is the observed value, is the observation equation, is the prior estimate of the inertial measurement state;

[0122] ,

[0123] in, is the error value, that is, the difference between the observed value and the actual measured value, is the actual measured value, is the observed value;

[0124] ,

[0125] in, is the mean bias of the inertial measurement unit, N is the size of the sliding window, i is the index of each data point in the sliding window, k is the present moment, is the difference between the observed value and the actual measured value.

[0126] For the real-time dynamic differential positioning system, the formula is expressed as:

[0127] ,

[0128] in, is the observed value, is the observation equation, is the prior estimate of the inertial measurement state;

[0129] ,

[0130] in, is the error value, that is, the difference between the observed value and the actual measured value, is the actual measured value, is the observed value;

[0131] ,

[0132] in, is the mean deviation of the real-time dynamic differential positioning system, N is the size of the sliding window, i is the index of each data point in the sliding window, k is the present moment, is the difference between the observed value and the actual measured value.

[0133] The Kalman gain acts as a weight coefficient, which can balance the weight distribution between the predicted value and the measured value. The Kalman gain is obtained based on the prediction covariance matrix, the Jacobian matrix of the state observation matrix, and the observation noise covariance matrix. The formula is as follows:

[0134] For the Inertial Measurement Unit:

[0135] ,

[0136] in, is the Kalman gain of the IMU, is the predicted covariance matrix of the IMU, is the Jacobian matrix of the IMU state observation matrix, is the observation noise covariance matrix of the IMU, T is the transpose of the matrix.

[0137] For real-time dynamic differential positioning system:

[0138] ,

[0139] in, is the Kalman gain of RTK, is the prediction covariance matrix of RTK, is the Jacobian matrix of the RTK state observation matrix, is the RTK observation noise covariance matrix.

[0140] Furthermore, based on the difference between the observed value and the actual measured value, the deviation mean and the Kalman gain, the state prior estimation is corrected to obtain the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit, which are specifically expressed as follows:

[0141] For the Inertial Measurement Unit:

[0142] ,

[0143] in, is the a posteriori estimate of the state of the inertial measurement unit, is the prior estimate of the inertial measurement state, is the Kalman gain, is the actual measured value, is the observed value, is the mean deviation.

[0144] For real-time dynamic differential positioning system:

[0145] ,

[0146] in, is the state posterior estimate of the real-time dynamic differential positioning system, is the prior estimate of the real-time dynamic differential positioning state, is the Kalman gain, is the actual measured value, is the observed value, is the mean deviation.

[0147] Furthermore, based on the Kalman gain and the Jacobian matrix of the state observation matrix, the prediction covariance matrix is ​​adjusted to obtain the posterior covariance matrices of the real-time dynamic differential positioning system and the inertial measurement unit, which are specifically expressed as follows:

[0148] For the Inertial Measurement Unit:

[0149] ,

[0150] in, is the posterior covariance matrix of the inertial measurement unit, I is the identity matrix, is the Kalman gain, is the Jacobian matrix of the state observation matrix, is the predicted covariance matrix of the inertial measurement unit.

[0151] For real-time dynamic differential positioning system:

[0152] ,

[0153] in, is the posterior covariance matrix of the real-time dynamic differential positioning system, I is the identity matrix, is the Kalman gain, is the Jacobian matrix of the state observation matrix, is the prediction covariance matrix of the real-time dynamic differential positioning system.

[0154] S4, based on the difference of the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit, a smoothing factor is introduced to adaptively adjust the weight factor of the previous moment, and the weight factors of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment are obtained respectively, and the weight factors are normalized to obtain the fusion weight;

[0155] Based on the fusion weight, the state posterior estimates of the real-time dynamic differential positioning system and the inertial measurement unit are adaptively weighted and fused to obtain the fused state estimate; the posterior covariance matrix is ​​adaptively weighted and fused to obtain the fusion covariance, and the agricultural robot is positioned based on the fusion state estimate and the fusion covariance.

[0156] Before performing adaptive weighted fusion processing of data, the IMU and RTK data must first be preprocessed to make the data format consistent and the timing synchronized to ensure that the IMU and RTK data are synchronized in time.

[0157] Specifically, in this step, based on the difference of the state a posteriori estimation of the real-time dynamic differential positioning system and the inertial measurement unit, a smoothing factor is introduced to adaptively adjust the weight factor of the previous moment, and the weight factors of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment are obtained respectively. The steps are:

[0158] If the difference between the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment is greater than the set adjustment threshold, the smoothing factor at the previous moment is subtracted from the specific adjustment variable;

[0159] Otherwise, the smoothing factor at the previous moment is added to the specific adjustment variable to obtain the smoothing factor at the current moment.

[0160] Based on the current moment smoothing factor and the difference between the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment, the weight factor of the previous moment is adaptively adjusted to obtain the weight factors of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment. , , the formula is:

[0161] ,

[0162] in, is the adjustment threshold of the smoothing factor, is the current moment smoothing factor, is the smoothing factor at the previous moment, β is the adjustment variable.

[0163] If the difference between the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit is greater than the set adjustment threshold, it indicates that the current environment or sensor state has changed, and the weight of one party needs to be increased to compensate for the drift of the other party.

[0164] ,

[0165] in, is the inertial measurement unit at the last moment, k -1 moment weight factor, It is the real-time dynamic differential positioning system at the last moment. k -1 moment weight factor, is the current moment smoothing factor, is the a posteriori estimate of the state of the inertial measurement unit, It is the state posterior estimate of the real-time dynamic differential positioning system.

[0166] By introducing a smoothing factor, the weight factors of IMU and RTK are adjusted. , , thus striking a balance between the responsiveness of the IMU and the accuracy of the RTK.

[0167] The weight factors of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment are normalized to obtain the fusion weight, which is expressed as follows:

[0168] ,

[0169] in, is the fusion weight of the inertial measurement unit, is the fusion weight of the real-time dynamic differential positioning system, is the weight factor of the real-time dynamic differential positioning system, is the weight factor of the inertial measurement unit.

[0170] Normalization is performed so that the sum of the weights of the two is 1, and the fusion weight is obtained, which can ensure that the contribution ratio of IMU and RTK matches the weight factor after adaptive adjustment, so as to find a reasonable balance between estimation accuracy and real-time response.

[0171] Based on the fusion weight, the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit is adaptively weighted and fused to obtain the fusion state estimation. , specifically:

[0172] ,

[0173] in, is the fusion weight of the inertial measurement unit, is the fusion weight of the real-time dynamic differential positioning system, is the a posteriori estimate of the state of the inertial measurement unit, It is the state posterior estimate of the real-time dynamic differential positioning system.

[0174] The posterior covariance matrix of the real-time dynamic differential positioning system and the inertial measurement unit is adaptively weighted and fused to obtain the fused covariance , specifically:

[0175] ,

[0176] in, is the a posteriori covariance matrix of the inertial measurement unit, is the posterior covariance matrix of the real-time dynamic differential positioning system.

[0177] By adaptively weighted fusion of IMU and RTK, a global state estimate with both dynamic response and accuracy is obtained, and the fused covariance represents the overall uncertainty of the state estimate. The fused covariance matrix reflects the respective confidence levels of IMU and RTK.

[0178] The agricultural robot is positioned based on fused state estimation and fused covariance, which serves as the input data for the next state prior estimation based on the dynamic model. It can make full use of the advantages of IMU and RTK data and improve the accuracy and robustness of state estimation.

[0179] The invention provides a weighted fusion agricultural robot positioning method based on deviation filtering, which can eliminate the systematic deviation in the observation error, respond to the actual observation value more accurately, and improve the effect of state estimation.

[0180] It should be further explained that the present invention adopts an adaptive weighted approach to perform data fusion, uses incremental optimization, and only optimizes the newly added smoothing factor. The computational complexity is relatively low, and the time deviation and asynchronous acquisition problems that may exist in the data of each sensor can be well handled. The smoothing factor value is dynamically adjusted according to the real-time performance of the system. At the same time, considering that the addition of the smoothing factor will increase the amount of calculation, it may have a certain impact on the real-time performance compared to the original algorithm. In order to ensure real-time performance, the smoothing factor is only updated once within a certain time interval, rather than at each time step.

[0181] During the entire data processing process, the IMU will reset its initial state value in each processing cycle to correct the estimated deviation of the IMU, thereby maintaining a high estimation accuracy in long-term operation. For the inherent noise and drift of the IMU sensor, long-term use will lead to cumulative errors. In order to eliminate these errors, the IMU can be reinitialized. For this, a fixed time parameter variable is set. When the step size of the IMU data reaches the set time value, the IMU data is reset. In addition, an error threshold is set. When the error value of the IMU data is greater than or equal to the error threshold, it indicates that the IMU data has drifted or the cumulative error is too large. At this time, the IMU data is reset. During the reset process, the position coordinates provided by RTK are used as the initial coordinates to restart the IMU data collection. In this way, the accumulated error of the IMU can be effectively prevented from having too much impact on the overall system, ensuring the accuracy and reliability of the data.

[0182] The present invention also provides a weighted fusion agricultural robot positioning system based on deviation filtering, comprising:

[0183] Data acquisition module: used to obtain the actual measurement values ​​of the position and motion posture of the agricultural robot at different times based on the real-time dynamic differential positioning system and the inertial measurement unit, and obtain the real-time dynamic differential positioning state data and the inertial measurement state data accordingly;

[0184] State prediction module: used to input the acquired real-time dynamic differential positioning state data and inertial measurement state data into the dynamic model, and obtain the real-time dynamic differential positioning state prior estimation and inertial measurement state prior estimation at the current moment respectively; based on the state covariance matrix at the previous moment, the Jacobian matrix of the state transfer matrix and the covariance matrix of the process noise, the corresponding prediction covariance matrix at the current moment is obtained;

[0185] State update module: used to input the real-time dynamic differential positioning state prior estimate and the inertial measurement state prior estimate into the dynamic model respectively, and obtain the observation value respectively; calculate the difference between the observation value and the actual measurement value, use the sliding window to traverse the historical moments, obtain several differences and then average them, and obtain the deviation mean of the real-time dynamic differential positioning system and the inertial measurement unit respectively; based on the prediction covariance matrix, the Jacobian matrix of the state observation matrix and the observation noise covariance matrix, the Kalman gain of the real-time dynamic differential positioning system and the inertial measurement unit are obtained respectively; based on the difference between the observation value and the actual measurement value, the deviation mean and the Kalman gain, the state prior estimate is corrected to obtain the state posterior estimate of the real-time dynamic differential positioning system and the inertial measurement unit respectively; based on the Kalman gain and the Jacobian matrix of the state observation matrix, the prediction covariance matrix is ​​adjusted to obtain the posterior covariance matrix of the real-time dynamic differential positioning system and the inertial measurement unit respectively;

[0186] Adaptive weighted fusion module: It is used to introduce a smoothing factor to adaptively adjust the weight factor of the previous moment based on the difference of the posterior state estimate of the real-time dynamic differential positioning system and the inertial measurement unit, and obtain the weight factors of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment, respectively, and normalize them to obtain the fusion weight; based on the fusion weight, the posterior state estimate of the real-time dynamic differential positioning system and the inertial measurement unit is adaptively weighted and fused to obtain the fusion state estimate; the posterior covariance matrix is ​​adaptively weighted and fused to obtain the fusion covariance, and the agricultural robot is positioned based on the fusion state estimate and the fusion covariance.

Claims

1. A weighted fusion agricultural robot positioning method based on deviation filtering, characterized in that: The following steps are involved: S1. Based on the real-time dynamic differential positioning system and the inertial measurement unit, the actual measurement values ​​of the position and motion posture of the agricultural robot at different times are respectively obtained, and the real-time dynamic differential positioning state data and the inertial measurement state data are correspondingly obtained; S2, inputting the acquired real-time dynamic differential positioning state data and inertial measurement state data into the dynamics model, and obtaining a priori estimation of the real-time dynamic differential positioning state and a priori estimation of the inertial measurement state at the current moment respectively; Based on the state covariance matrix of the previous moment, the Jacobian matrix of the state transfer matrix and the covariance matrix of the process noise, the corresponding prediction covariance matrix of the current moment is obtained; S3, inputting the real-time dynamic differential positioning state prior estimation and the inertial measurement state prior estimation into the dynamic model respectively, and obtaining the observation values ​​respectively; calculating the difference between the observation value and the actual measurement value, using a sliding window to traverse the historical moments, and averaging the obtained several differences, and obtaining the deviation mean of the real-time dynamic differential positioning system and the inertial measurement unit respectively; Based on the prediction covariance matrix, the Jacobian matrix of the state observation matrix and the observation noise covariance matrix, the Kalman gains of the real-time dynamic differential positioning system and the inertial measurement unit are obtained respectively; Based on the difference between the observed value and the actual measured value, the deviation mean and the Kalman gain, the state prior estimation is corrected to obtain the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit respectively; Based on the Kalman gain and Jacobian matrix of the state observation matrix, the prediction covariance matrix is ​​adjusted to obtain the posterior covariance matrix of the real-time dynamic differential positioning system and the inertial measurement unit respectively; S4, based on the difference of the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit, a smoothing factor is introduced to adaptively adjust the weight factor of the previous moment, and the weight factors of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment are obtained respectively, and the weight factors are normalized to obtain the fusion weight; Based on the fusion weight, the state posterior estimates of the real-time dynamic differential positioning system and the inertial measurement unit are adaptively weighted and fused to obtain the fused state estimate; the posterior covariance matrix is ​​adaptively weighted and fused to obtain the fusion covariance, and the agricultural robot is positioned based on the fusion state estimate and the fusion covariance.

2. The weighted fusion agricultural robot positioning method based on deviation filtering according to claim 1 is characterized in that: Based on the difference between the state posterior estimates of the real-time dynamic differential positioning system and the inertial measurement unit, a smoothing factor is introduced to adaptively adjust the weight factor of the previous moment, and the weight factors of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment are obtained respectively. The steps are as follows: If the difference between the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment is greater than the set adjustment threshold, the smoothing factor at the previous moment is subtracted from the specific adjustment variable; Otherwise, add the previous moment smoothing factor to the specific adjustment variable to get the current moment smoothing factor; Based on the current moment smoothing factor and the difference between the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment, the weight factor of the previous moment is adaptively adjusted to obtain the weight factors of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment. , , the formula is: ， in, is the adjustment threshold of the smoothing factor, is the current moment smoothing factor, is the smoothing factor at the previous moment, β is the adjustment variable; ， in, is the inertial measurement unit at the last moment, k -1 moment weight factor, It is the real-time dynamic differential positioning system at the last moment. k -1 moment weight factor, is the current moment smoothing factor, is the a posteriori estimate of the state of the inertial measurement unit, It is the state posterior estimate of the real-time dynamic differential positioning system.

3. The weighted fusion agricultural robot positioning method based on deviation filtering according to claim 1 is characterized in that: In the S3, the real-time dynamic differential positioning state prior estimation and the inertial measurement state prior estimation are respectively input into the dynamic model to obtain observation values ​​respectively; the difference between the observation value and the actual measurement value is calculated, and the historical moments are traversed using a sliding window to obtain a number of differences and then average them to obtain the deviation mean of the real-time dynamic differential positioning system and the inertial measurement unit respectively. For an inertial measurement unit, the formula is expressed as: ， in, is the observation value of the inertial measurement unit, is the observation equation, is the prior estimate of the inertial measurement state; ， in, is the error value, that is, the difference between the observed value and the actual measured value, is the actual measured value, is the observed value; ， in, is the mean bias of the inertial measurement unit, N is the size of the sliding window, i is the index of each data point in the sliding window, k is the present moment, is the difference between the observed value and the actual measured value; For the real-time dynamic differential positioning system, the formula is expressed as: ， in, is the observation value of the real-time dynamic differential positioning system, is the observation equation, It is a priori estimate of the real-time dynamic differential positioning state; ， in, is the error value, that is, the difference between the observed value and the actual measured value, is the actual measured value, is the observed value; ， in, is the mean deviation of the real-time dynamic differential positioning system, N is the size of the sliding window, i is the index of each data point in the sliding window, k is the present moment, is the difference between the observed value and the actual measured value.

4. The weighted fusion agricultural robot positioning method based on deviation filtering according to claim 3 is characterized in that: Based on the difference between the observed value and the actual measured value, the deviation mean and the Kalman gain, the state prior estimation is corrected to obtain the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit, which are specifically expressed as follows: For the Inertial Measurement Unit: ， in, is the a posteriori estimate of the state of the inertial measurement unit, is the prior estimate of the inertial measurement state, is the Kalman gain, is the actual measured value, is the observed value, is the mean of the deviations; For real-time dynamic differential positioning system: ， in, is the state posterior estimate of the real-time dynamic differential positioning system, is the prior estimate of the real-time dynamic differential positioning state, is the Kalman gain, is the actual measured value, is the observed value, is the mean deviation.

5. The weighted fusion agricultural robot positioning method based on deviation filtering according to claim 2 is characterized in that: In S4, the weight factors of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment are normalized to obtain a fusion weight, which is expressed as follows: ， in, is the fusion weight of the inertial measurement unit, is the fusion weight of the real-time dynamic differential positioning system, is the weight factor of the real-time dynamic differential positioning system, is the weight factor of the inertial measurement unit; Based on the fusion weight, the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit is adaptively weighted and fused to obtain the fusion state estimation. , specifically: ， in, is the fusion weight of the inertial measurement unit, is the fusion weight of the real-time dynamic differential positioning system, is the a posteriori estimate of the state of the inertial measurement unit, It is the state posterior estimate of the real-time dynamic differential positioning system; The posterior covariance matrix of the real-time dynamic differential positioning system and the inertial measurement unit is adaptively weighted and fused to obtain the fused covariance , specifically: ， in, is the a posteriori covariance matrix of the inertial measurement unit, is the posterior covariance matrix of the real-time dynamic differential positioning system.

6. The weighted fusion agricultural robot positioning method based on deviation filtering according to claim 1 is characterized in that: In S2, based on the acquired real-time dynamic differential positioning state data and inertial measurement state data, the acquired state data is input into the dynamic model to obtain the prior estimation of the real-time dynamic differential positioning state and the prior estimation of the inertial measurement state at the current moment, specifically: The acquired real-time dynamic differential positioning state data and inertial measurement state data are input into the dynamic model to obtain the state estimation data of the real-time dynamic differential positioning system and the inertial measurement unit at the previous moment. Based on the state estimation data at the previous moment, the dynamic model is input to obtain the prior estimation of the real-time dynamic differential positioning state and the prior estimation of the inertial measurement state at the current moment. The formula is expressed as follows: For the Inertial Measurement Unit: ， in, is the state estimation data of the inertial measurement unit at the previous moment, i.e., moment k-1, is the control input at the previous moment, i.e., moment k-1; To obtain the state estimation data of the current moment according to the k-1 moment, that is, the prior estimation of the inertial measurement state; For real-time dynamic differential positioning system: ， in, It is to obtain the prior estimate of the real-time dynamic differential positioning state at the current moment based on the k-1 moment. is the state estimation data of the real-time dynamic differential positioning system at time k-1, is the control input at time k-1.

7. The weighted fusion agricultural robot positioning method based on deviation filtering according to claim 1 is characterized in that: In S2, based on the state covariance matrix at the previous moment, the Jacobian matrix of the state transfer matrix and the covariance matrix of the process noise, the prediction covariance matrix at the current moment is obtained, and the formula is expressed as: For the Inertial Measurement Unit: ， in, is the predicted covariance matrix of the inertial measurement unit at the current moment, is the covariance matrix of the inertial measurement unit at the previous moment, i.e., moment k-1, is the Jacobian matrix of the state transfer matrix of the inertial measurement unit at time k-1, is the covariance matrix of the process noise at time k-1; For real-time dynamic differential positioning system: ， in, is the prediction covariance matrix of the real-time dynamic differential positioning system at the current moment, is the covariance matrix of the real-time dynamic differential positioning system at the previous moment, i.e., moment k-1, is the Jacobian matrix of the state transfer matrix of the real-time dynamic differential positioning system at time k-1, is the covariance matrix of the process noise at time k-1.

8. The weighted fusion agricultural robot positioning method based on deviation filtering according to claim 1 is characterized in that: In S3, the prediction covariance matrix is ​​adjusted based on the Kalman gain and the Jacobian matrix of the state observation matrix to obtain the posterior covariance matrix of the real-time dynamic differential positioning system and the inertial measurement unit, which is specifically expressed as follows: For the Inertial Measurement Unit: ， in, is the a posteriori covariance matrix of the inertial measurement unit, I is the identity matrix, is the Kalman gain, is the Jacobian matrix of the state observation matrix, is the predicted covariance matrix of the inertial measurement unit; For real-time dynamic differential positioning system: ， in, is the posterior covariance matrix of the real-time dynamic differential positioning system, I is the identity matrix, is the Kalman gain, is the Jacobian matrix of the state observation matrix, is the prediction covariance matrix of the real-time dynamic differential positioning system.

9. The weighted fusion agricultural robot positioning method based on deviation filtering according to claim 1 is characterized in that: The dynamic model of the real-time dynamic differential positioning system and the inertial measurement unit is expressed as follows: ， ， in, yes k +1 status data at the moment, yes k The state vector of the real-time dynamic differential positioning system or inertial measurement unit, i.e., the state data; yes k The control input at the moment, is the state equation for state data and control input, yes k The process noise at each moment; yes k +1 measured value at time, yes k About status data at all times The observation equation is yes k The measurement noise at the moment.

10. A weighted fusion agricultural robot positioning system based on deviation filtering, characterized in that: include: Data acquisition module: used to obtain the actual measurement values ​​of the position and motion posture of the agricultural robot at different times based on the real-time dynamic differential positioning system and the inertial measurement unit, and obtain the real-time dynamic differential positioning state data and the inertial measurement state data accordingly; State prediction module: input the acquired real-time dynamic differential positioning state data and inertial measurement state data into the dynamic model to obtain the real-time dynamic differential positioning state prior estimation and inertial measurement state prior estimation at the current moment respectively; based on the state covariance matrix at the previous moment, the Jacobian matrix of the state transfer matrix and the covariance matrix of the process noise, obtain the corresponding prediction covariance matrix at the current moment; State update module: used to input the real-time dynamic differential positioning state prior estimation and the inertial measurement state prior estimation into the dynamic model respectively to obtain observation values ​​respectively; Calculate the difference between the observed value and the actual measured value, use a sliding window to traverse the historical moments, obtain a number of differences and then calculate the average, and obtain the deviation mean of the real-time dynamic differential positioning system and the inertial measurement unit respectively; based on the prediction covariance matrix, the Jacobian matrix of the state observation matrix and the observation noise covariance matrix, obtain the Kalman gain of the real-time dynamic differential positioning system and the inertial measurement unit respectively; based on the difference between the observed value and the actual measured value, the deviation mean and the Kalman gain, correct the state prior estimate, and obtain the state posterior estimate of the real-time dynamic differential positioning system and the inertial measurement unit respectively; based on the Kalman gain and the Jacobian matrix of the state observation matrix, adjust the prediction covariance matrix, and obtain the posterior covariance matrix of the real-time dynamic differential positioning system and the inertial measurement unit respectively; Adaptive weighted fusion module: It is used to introduce a smoothing factor to adaptively adjust the weight factor of the previous moment based on the difference of the state posterior estimation of the real-time dynamic differential positioning system and the inertial measurement unit, and obtain the weight factors of the real-time dynamic differential positioning system and the inertial measurement unit at the current moment, and normalize them to obtain the fusion weight; Based on the fusion weight, the state posterior estimates of the real-time dynamic differential positioning system and the inertial measurement unit are adaptively weighted and fused to obtain the fused state estimate; the posterior covariance matrix is ​​adaptively weighted and fused to obtain the fusion covariance, and the agricultural robot is positioned based on the fusion state estimate and the fusion covariance.

Images