A combined navigation and positioning method and system for unmanned agricultural machinery

Abstract

The present invention provides an integrated navigation and positioning method and system for unmanned agricultural machinery, relating to the field of satellite navigation and positioning technology. The method comprises: step S1: receiving and processing sensor data; step S2: setting the initial state of the integrated navigation system and stabilizing it; step S3: identifying and constraining the motion state; step S4: fault-tolerant filtering and estimating the navigation state; and step S5: closed-loop feedback of the navigation state and output of navigation results. The present invention significantly enhances the fault tolerance and stability of the integrated navigation system, providing continuous, stable, and high-precision positioning results for unmanned agricultural machinery.

Description

Technical Field

[0001] The present invention relates to the field of satellite navigation and positioning technology, and in particular to a combined navigation and positioning method and system for unmanned agricultural machinery. Background Art

[0002] With the advancement of agricultural modernization, demand for autonomous production operations by unmanned agricultural machinery is growing. The key to this is obtaining stable, continuous, and highly accurate positioning data. Currently, while the Global Navigation Satellite System (GNSS) provides accurate three-dimensional coordinates and velocity information around the clock, it is susceptible to environmental interference and has a low data update frequency. While the Inertial Navigation System (INS) offers strong autonomy and excellent anti-interference capabilities, it suffers from error accumulation. To address this, the GNSS / INS combined navigation system has emerged. By combining the advantages of both, it significantly improves dynamic navigation and positioning performance in complex environments, enabling more accurate, continuous, and reliable navigation services. With the advancement of microelectromechanical systems (MEMS) technology, MEMS inertial sensors (MEMS-IMUs), with their compact size and low cost, have become an ideal choice for GNSS / INS combined navigation systems in agricultural scenarios, demonstrating broad application prospects.

[0003] However, in actual agricultural operations, GNSS / INS integrated navigation systems face numerous challenges. On the one hand, the vibration and complex motion of agricultural machinery platforms can easily lead to unstable bias estimates of low-cost IMUs, causing rapid divergence in INS navigation results and reducing the accuracy of integrated navigation. On the other hand, the complexity of the operating environment and hardware failures can lead to errors and erroneous measurements, affecting the accuracy and reliability of the filtering results. Existing GNSS / INS integrated navigation technologies often use extended Kalman filters (EKFs) for information fusion, but this fails to fully address these issues, limiting their application in unmanned agricultural machinery operations and making it difficult to meet the requirements of unmanned agricultural machinery for continuous, stable, and high-precision navigation and positioning. Summary of the Invention

[0004] To address the aforementioned technical issues, the present invention provides an integrated navigation and positioning method and system for unmanned agricultural machinery. The method uses low-pass filtering technology to process data collected by an inertial navigation system (INS) to eliminate high-frequency noise interference introduced by vehicle vibration. Secondly, constraints are imposed on the INS navigation results based on the vehicle's motion state. Specifically, zero-speed correction is implemented when the vehicle is stationary, while non-holonomic constraints (NHC) are applied when the vehicle is in motion. NHC noise is dynamically adjusted based on the vehicle's motion speed and the real-time calculated rate of change of heading. Finally, a fault detection and fault tolerance mechanism is embedded in the filtering and estimation process to reduce the impact of system errors and abnormal interference on navigation state estimation. This method significantly enhances the fault tolerance and stability of the integrated navigation system, providing continuous, stable, and high-precision positioning results for unmanned agricultural machinery.

[0005] In order to achieve the above object, the present invention adopts the following technical solutions:

[0006] A combined navigation and positioning method for unmanned agricultural machinery includes the following steps:

[0007] Step S1: collecting GNSS positioning result data and IMU raw data, and performing data filtering preprocessing on the IMU raw data using a low-pass filter suitable for agricultural machinery motion scenarios; GNSS stands for Global Navigation Satellite System, and IMU stands for Micro Electro Mechanical System Inertial Sensor;

[0008] Step S2: Initialize the integrated navigation system state according to the GNSS positioning result, use the three-dimensional position and velocity of the GNSS positioning result as the initial position and velocity, and use the three-dimensional position standard deviation as the judgment condition. When it is less than the threshold, initialize the integrated navigation system state, and perform INS inertial dead reckoning and Kalman filtering in the initialized state; INS represents an inertial navigation system;

[0009] Step S3: Determine whether the agricultural machine is in a stationary or moving state based on the result of the INS inertial navigation system. Perform zero-speed correction when the machine is in a stationary state, and perform adaptive nonholonomic constraints when the machine is in a moving state. For scenarios involving uniform motion of the agricultural machine, dynamically adjust the adaptive nonholonomic constraint noise using an adaptive sliding window setting method.

[0010] Step S4: using the linearized extended Kalman filter measurement update equation to optimally estimate the state quantity and covariance of the integrated navigation system to obtain a system state error correction value;

[0011] Step S5: Feedback the system state error correction value of step S4 to the integrated navigation system, correct the inertial navigation error, and output the integrated navigation positioning result in real time.

[0012] The present invention also provides a combined navigation and positioning system for unmanned agricultural machinery, comprising the following modules:

[0013] The acquisition module collects GNSS positioning result data and IMU raw data, and uses a low-pass filter suitable for agricultural machinery movement scenarios to perform data filtering preprocessing on the IMU raw data. GNSS stands for Global Navigation Satellite System, and IMU stands for Micro Electro Mechanical System Inertial Sensor.

[0014] The dead reckoning and filtering module initializes the integrated navigation system state based on the GNSS positioning results. The three-dimensional position and velocity of the GNSS positioning results are used as the initial position and velocity, and the three-dimensional position standard deviation is used as the judgment condition. When it is less than the threshold, the integrated navigation system state is initialized and INS inertial dead reckoning and Kalman filtering are performed in the initialized state. INS stands for inertial navigation system.

[0015] The adjustment module determines the stationary or moving state of the agricultural machinery based on the results of the INS inertial navigation. It performs zero-speed correction when the machinery is stationary and applies adaptive non-holonomic constraints when the machinery is in motion. In addition, it uses an adaptive sliding window setting method to dynamically adjust the adaptive non-holonomic constraint noise for scenarios where the machinery is in uniform motion.

[0016] The estimation module uses the linearized extended Kalman filter measurement update equation to optimally estimate the state quantity and covariance of the integrated navigation system and obtain the system state error correction value;

[0017] The output module feeds back the system state error correction value to the integrated navigation system, corrects the inertial navigation error, and outputs the integrated navigation positioning result in real time.

[0018] The present invention also provides an electronic device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps of the above-mentioned combined navigation and positioning method for unmanned agricultural machinery are implemented.

[0019] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the above-mentioned combined navigation and positioning method for unmanned agricultural machinery.

[0020] Beneficial effects:

[0021] The present invention comprehensively considers the application scenario characteristics of unmanned agricultural machinery, reduces the impact of vehicle body vibration on the high-frequency noise of inertial navigation observation data through a low-pass data filtering method suitable for agricultural machinery scenarios, takes into account the movement state of the agricultural machinery to constrain the INS mechanical arrangement results, and adopts an adaptive sliding window setting method when the agricultural machinery is in motion. The NHC noise is flexibly adjusted based on the movement speed and the heading change obtained according to the movement state of the agricultural machinery. Finally, by adding fault detection and fault tolerance mechanisms to the filter estimation, the impact of combined system errors and abnormal interference on state estimation is further weakened, the fault tolerance and stability of the combined navigation method are enhanced, and it can provide continuous, stable and high-precision positioning results for unmanned agricultural machinery. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 This is a flow chart of the combined navigation and positioning method for unmanned agricultural machinery of the present invention.

[0023] Figure 2a , Figure 2b It is a comparison diagram of the combined navigation result trajectory diagram; among them, Figure 2a For the combined navigation positioning result using only fixed NHC noise constraints, Figure 2b This is the combined navigation positioning result of the present invention, where N represents north and E represents east.

[0024] Figure 3 This is a schematic diagram of the combined navigation and positioning device for unmanned agricultural machinery of the present invention. DETAILED DESCRIPTION

[0025] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention is further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other. In the process of data preprocessing, the present invention takes into account the special interference of agricultural machinery carrier vibration, and reduces the impact of vibration on the original observation data of low-cost inertial sensors through data filtering; in the process of information fusion, the combined navigation performance is further enhanced by adding carrier motion state constraints, including zero-speed correction and non-completeness constraints, and considering the vehicle turning state, the NHC adaptive factor is introduced to reduce the impact of lateral speed noise on non-completeness constraints; finally, the combined system error and abnormal interference are considered in the filter estimation, and the gross error detection and fault tolerance mechanism are adopted to enhance the fault tolerance and stability of the filter.

[0026] like Figure 1 As shown, the present invention provides a combined navigation and positioning method for unmanned agricultural machinery, comprising the following steps:

[0027] Step S1, sensor data reception and processing, includes:

[0028] The real-time sensor data stream is collected, decoded, and preprocessed, including GNSS three-dimensional position, velocity, and IMU raw data. Only the GNSS three-dimensional position and velocity data are decoded and stored, and their position, velocity, corresponding covariance, and positioning status information are recorded. The IMU raw data is decoded, stored, and preprocessed, including three-axis accelerometer data and three-axis gyroscope data. To reduce the impact of agricultural machinery carrier vibration on IMU data, the acceleration and angular velocity data are further preprocessed by data filtering. A low-pass filter suitable for agricultural machinery motion scenarios is designed, and the low-pass filter cutoff frequency is selected to be lower than the agricultural machinery vibration frequency. The formula is as follows:

[0029] (1)

[0030] in, Indicates the IMU results after this filtering, including three-axis accelerometer and three-axis gyroscope data, Indicates the IMU result after the last filtering, Indicates the original sampling value of IMU, = represents the dynamic filter factor, and n represents the number of iterations. Since the three-axis gyroscope and three-axis accelerometer in the IMU sensor have different sensitivities to agricultural machinery vibration, and the vibration state of agricultural machinery at different speeds has different effects on IMU data, the dynamic filter factor needs to be set as follows:

[0031] (2)

[0032] in, is the velocity increment factor, and They are the accelerometer X-axis velocity increment output and Y-axis velocity increment output, is the IMU epoch, is the filtering window time, is the time constant of the first-order filter, represents the cutoff frequency, and are the accelerometer filter factor and the gyroscope filter factor, 、 and All are set by experience.

[0033] Step S2, initial state setting and stabilization of the integrated navigation system, includes:

[0034] Initialize the integrated navigation system state according to the GNSS positioning results, and use the real-time received GNSS three-dimensional position and speed as the initial position and speed of the integrated navigation system. At the same time, in order to ensure the reliability of the initial state, the standard deviation of the GNSS three-dimensional position result is used. As the judgment condition, if it is less than the set standard deviation threshold When the integrated navigation system state initialization process is performed, the integrated navigation system is initialized with the GNSS positioning result at that moment as the initial value, and this is used as the starting state for INS inertial navigation calculation and extended Kalman filtering (involving the construction of state equations and observation equations, which need to be initialized for subsequent state parameter estimation, and the estimation process corresponds to step S4).

[0035] Step S3, motion state identification and constraint, includes:

[0036] Step S3.1: Determine the motion state of the agricultural machinery based on the results of INS inertial navigation and IMU raw data, and set the motion discrimination parameters. , and its calculation formula is:

[0037] (3)

[0038] Among them, m represents the number of The number of IMU epoch sliding windows, represents the IMU epoch, Indicates that the current epoch carrier is in the navigation coordinate system ( The gravitational acceleration under the system, ; x, y, z represent The three coordinate axes of the system, Represents the specific force measurement of the accelerometer sensor at the current epoch, is the accelerometer noise used for zero-speed detection determination.

[0039] At the same time, set the zero speed detection threshold. If the zero speed detection threshold is less than the zero speed detection threshold, it is considered to be in a stationary state and the zero speed correction process is performed, that is, step S3.2 is performed. If it is greater than or equal to the zero-speed detection threshold, it is considered to be in motion and an adaptive non-holonomic constraint (NHC) is performed, i.e., step S3.3 is performed;

[0040] Step S3.2 When it is determined that the agricultural machine is in a stationary state, the static zero speed correction (ZUPT) process is performed, and it is considered that the current carrier coordinate system ( The velocity under the system is 0, and the heading angle difference with the previous position is 0, that is:

[0041] (4)

[0042] in, for System download speed, 、 and They are The speed of the carrier in the front, right and bottom directions can be calculated based on the above constraints, that is, formula (4), and the speed constraint observation equation in the static state can be constructed:

[0043] (5)

[0044] in, is the observation innovation vector of ZUPT, Inertial navigation The carrier velocity under the system is generally not zero due to the existence of inertial guidance error. is the state parameter of the integrated navigation system, which is 21-dimensional. To measure noise, is the ZUPT measurement matrix, and:

[0045] (6)

[0046] in, represents a unit vector with 3 rows and 3 columns, and It represents the zero vector of 3 rows and 3 columns and 3 rows and 15 columns;

[0047] Step S3.3 If the agricultural machine is in motion, the adaptive nonholonomic constraint process is performed, assuming that the agricultural machine is in motion. The forward velocity in the system is the carrier motion velocity, while the lateral and vertical velocities are zero, that is:

[0048] (7)

[0049] in, for System download speed, 、 and They are The speed of the carrier in the front, right and bottom directions is also obtained by the inertial navigation mechanism. The actual calculation can be calculated System download speed:

[0050] (8)

[0051] in, for System download speed, for Department and The attitude transformation matrix between the two systems, and the superscript T represents the transpose of the matrix.

[0052] Perform error disturbance analysis on the above formula and subtract the actual calculated speed from the theoretical true speed, then we have:

[0053] (9)

[0054] in, and Then it is the velocity error state quantity and attitude error state quantity in the n system, For antisymmetric matrix symbols, the velocity constraint observation equation can be constructed according to the above formula. Among them, NHC only targets the lateral velocity and vertical velocity, so the observation equation only takes the lateral component and the vertical component:

[0055] (10)

[0056] (11)

[0057] in, is the NHC velocity noise, and Represent the observation information vector and measurement matrix of NHC constraints, is the state parameter of the integrated navigation system, express Rows 2 and 3, columns 1 to 3, and It represents a zero vector with 2 rows and 3 columns and 2 rows and 12 columns.

[0058] When operating agricultural machinery, it is restricted by the special environment of farmland, and it is inevitable that it will sway left and right to varying degrees. In addition, factors such as agricultural machinery steering and road bumps will cause certain lateral velocity noise and vertical velocity noise. Therefore, NHC noise cannot be simply set to a fixed value and needs to be adaptively adjusted according to the movement state of the agricultural machinery. Here, NHC noise is adaptively adjusted according to the movement speed and steering angle of the agricultural machinery:

[0059] (12)

[0060] (13)

[0061] in, Indicates the rate of change of the steering angle of the agricultural machinery, is the NHC velocity noise matrix, and are the lateral velocity noise and the longitudinal velocity noise, respectively. is the heading angle change value, 、 is the set horizontal adaptive scale factor and vertical adaptive scale factor;

[0062] Agricultural machinery steering angle change rate Determined based on the degree of heading angle change within a certain window time:

[0063] (14)

[0064] (15)

[0065] in, is the estimated heading angle at the current moment, is the IMU epoch, N is the total amount of data in the window time, The window time for obtaining the heading angle is adjusted adaptively according to the carrier's motion speed. and Set the speed and time thresholds respectively, and obtain the window time for the heading angle Set the minimum threshold To avoid abnormal calculation caused by excessively high speed of agricultural machinery;

[0066] According to the above motion constraint equations, namely formula (4) and formula (7), motion state discrimination and corresponding constraints are performed after each INS inertial navigation calculation is completed, and the constrained error state quantity and its covariance are obtained, which are then fed back into the mechanical arrangement posture result.

[0067] Step S4, navigation state fault-tolerant filtering estimation, includes:

[0068] Step S4.1: Based on the linearized extended Kalman filter measurement update equation, the Kalman filter is updated for the state quantity and covariance of the integrated navigation system. At the same time, considering the abnormal interference of the real-time integrated navigation system, a gross error detection and fault tolerance mechanism is established. The current state of the integrated navigation system is determined based on the measurement innovation. is the difference between the actual observed value and the predicted value of the observed value:

[0069] (16)

[0070] Measurement innovation variance matrix The calculation formula is as follows:

[0071] (17)

[0072] in, The combined measurement updates the observation value. The measurement update matrix of the pine combination, is the state vector of the loosely combined system, is the normalized innovation vector, which can reflect the matching degree between the actual value of the observed innovation and the theoretical value. represents the measurement innovation value corresponding to the j-th observation, represents the variance value corresponding to the j-th observation, is the observation noise covariance matrix, is the k-time prediction error covariance matrix;

[0073] Step S4.2 Based on the normalized innovation vector The value of is used to perform adaptive adjustment of the measurement innovation variance, so as to control the influence of measurement innovation in measurement update and reduce the influence of integrated navigation system error and abnormal interference on state estimation; set the threshold ,when When a fault occurs, the adjusted measurement innovation variance is obtained. :

[0074] (18)

[0075] Step S4.3: Based on the adjusted measurement innovation variance Perform the Kalman filter measurement update process.

[0076] Step S5, closed-loop feedback of navigation status and output of navigation results, includes:

[0077] The system status parameters Feedback to the integrated navigation system realizes closed-loop feedback of integrated navigation errors. On the one hand, it is the feedback of position, velocity and attitude errors, correcting the corresponding state errors to the INS calculation results and outputting the results. On the other hand, it is the feedback of inertial sensor errors (accelerometer and gyroscope bias), using their error estimates to correct the inertial sensor output values, and performing subsequent INS calculations based on the corrected inertial sensor data.

[0078] like Figure 3 As shown, the present invention also provides a combined navigation and positioning system for unmanned agricultural machinery, including the following modules:

[0079] The acquisition module collects GNSS positioning result data and IMU raw data, and uses a low-pass filter suitable for agricultural machinery movement scenarios to perform data filtering preprocessing on the IMU raw data;

[0080] The dead reckoning and filtering module initializes the integrated navigation system state according to the GNSS positioning results. The three-dimensional position and velocity of the GNSS positioning results are used as the initial position and velocity, and the three-dimensional position standard deviation is used as the judgment condition. When it is less than the threshold, the integrated navigation system state is initialized, and INS inertial dead reckoning and Kalman filtering are performed in the initialized state.

[0081] The adjustment module determines the stationary or moving state of the agricultural machinery based on the results of the INS inertial navigation. It performs zero-speed correction when the machinery is stationary and applies adaptive non-holonomic constraints when the machinery is in motion. In addition, it uses an adaptive sliding window setting method to dynamically adjust the adaptive non-holonomic constraint noise for scenarios where the machinery is in uniform motion.

[0082] The estimation module uses the linearized extended Kalman filter measurement update equation to optimally estimate the state quantity and covariance of the integrated navigation system and obtain the system state error correction value;

[0083] The output module feeds back the system state error correction value to the integrated navigation system, corrects the inertial navigation error, and outputs the integrated navigation positioning result in real time.

[0084] The present invention also provides an electronic device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps of the above-mentioned combined navigation and positioning method for unmanned agricultural machinery are implemented.

[0085] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the above-mentioned combined navigation and positioning method for unmanned agricultural machinery.

[0086] Example:

[0087] A set of low-cost IMU and GNSS result data collected in practice was used to conduct a real-time integrated navigation simulation test in an agricultural machinery scenario. Based on the vibration frequency of the agricultural machinery during actual movement, this set of IMU data was simulated and processed to simulate the raw data output in the agricultural machinery operation scenario to verify the effectiveness of the present invention. The parameters involved in the specific processing process are set as follows:

[0088] Step 1: Obtain GNSS positioning results and IMU raw data, and preprocess the IMU data. The dynamic filter factor calculation window time is set to 5s, and the low-pass filter cutoff frequency is 15Hz.

[0089] Step 2: Set the position standard deviation threshold based on experience is 1.0m, when the GNSS three-dimensional position standard deviation , initialize the integrated navigation system state, and use the epoch position, velocity and attitude results as the initial values ​​to perform INS inertial navigation calculation and Kalman filter update;

[0090] Step 3: In motion state identification and constraint, for low-speed motion of agricultural machinery, set the number of IMU epoch sliding windows of motion judgment parameters to 10 (IMU sampling rate is 100Hz), and set the motion state parameter threshold to 50; in addition, when performing adaptive NHC constraint, set the lateral noise adaptive factor is 1.5, 1.0, the window value of the heading angle change value is being calculated and The thresholds are set to 2 m / s and 2 s, respectively, and the motion state recognition and corresponding motion constraints are performed with the above parameter settings;

[0091] Step 4: In the navigation state fault-tolerant filter estimation, set the threshold of the normalized innovation vector is 3.0;

[0092] Step 5: Based on the above parameter settings, the closed-loop feedback of the navigation status and the output of the navigation results are finally achieved.

[0093] The combined navigation positioning results of the present invention are compared with those of the combined navigation positioning results using only fixed NHC noise constraints. The corresponding combined navigation positioning trajectory diagram is shown in FIG. Figure 2a , Figure 2b As shown, Figure 2a For the combined navigation positioning result using only fixed NHC noise constraints, Figure 2b The combined navigation positioning result of the present invention is converted to the northeast sky coordinate system, where the horizontal and vertical coordinates represent the coordinate values ​​in the east and north directions respectively. Figure 2a , Figure 2b It can be seen that the combined navigation results of the present invention are smoother, and the low-pass filtering weakens the impact of vehicle vibration on the IMU. In addition, for the positioning results in the turning section, the adaptive NHC noise determination adopted by the present invention further enhances the navigation positioning accuracy in this section.

[0094] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Thus, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk drives, CD-ROMs, optical storage devices, etc.) containing computer-usable program code. The solutions in the embodiments of the present invention may be implemented using various computer languages, such as the object-oriented programming language Java and the interpreted scripting language JavaScript.

[0095] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowcharts and / or block diagrams, as well as combinations of processes and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowcharts and / or block diagrams. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0096] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0097] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 The steps for the function specified in one or more boxes.

[0098] Although the preferred embodiments of the present invention have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

[0099] Obviously, those skilled in the art may make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if such changes and modifications fall within the scope of the claims and their equivalents, the present invention is intended to include such changes and modifications.

Claims

1. A combined navigation and positioning method for unmanned agricultural machinery, characterized in that: The following steps are involved: Step S1: collecting GNSS positioning result data and IMU raw data, and performing data filtering preprocessing on the IMU raw data using a low-pass filter suitable for agricultural machinery movement scenarios; GNSS stands for Global Navigation Satellite System, and IMU stands for Micro Electro Mechanical System Inertial Sensor; Step S2: Initialize the integrated navigation system state according to the GNSS positioning result, use the three-dimensional position and velocity of the GNSS positioning result as the initial position and velocity, and use the three-dimensional position standard deviation as the judgment condition. When it is less than the threshold, initialize the integrated navigation system state, and perform INS inertial dead reckoning and Kalman filtering in the initialized state; INS represents an inertial navigation system; Step S3: Determine whether the agricultural machinery is in a stationary state or a moving state based on the result of the INS inertial navigation. Perform zero-speed correction in the stationary state and perform adaptive non-holonomic constraints in the moving state. For the uniform motion scenario of the agricultural machinery, dynamically adjust the adaptive non-holonomic constraint noise using an adaptive sliding window setting method, including: The motion state of the agricultural machinery is judged based on the results of INS inertial navigation and the original data of IMU, and the motion discrimination parameters are set; the zero-speed detection threshold is set. If the motion discrimination parameter is less than the zero-speed detection threshold, it is considered to be in a stationary state and the zero-speed correction process is performed; if the motion discrimination parameter is greater than or equal to the zero-speed detection threshold, it is considered to be in a moving state and an adaptive non-holonomic constraint is performed. The adaptive non-holonomic constraint noise is adaptively adjusted according to the motion speed and steering angle of the agricultural machinery to obtain the steering angle change rate of the agricultural machinery. : （14） in, is the estimated heading angle at the current moment, is the IMU epoch, N is the total amount of data in the window time, The window time for obtaining the heading angle is adjusted adaptively according to the carrier's motion speed. and Set speed and time thresholds respectively; Step S4: using the linearized extended Kalman filter measurement update equation to optimally estimate the state quantity and covariance of the integrated navigation system to obtain a system state error correction value; Step S5: Feedback the system state error correction value of step S4 to the integrated navigation system, correct the inertial navigation error, and output the integrated navigation positioning result in real time.

2. The combined navigation and positioning method for unmanned agricultural machinery according to claim 1, characterized in that: The step S1 comprises: Decode and store the GNSS positioning result data, and record its position, velocity, covariance and positioning status information; decode, store and pre-process the IMU raw data, including low-pass filtering of the three-axis accelerometer data and the three-axis gyroscope data.

3. The combined navigation and positioning method for unmanned agricultural machinery according to claim 1, characterized in that: The low-pass filter suitable for the agricultural machinery motion scenario in step S1 is: （1） in, Indicates the IMU results after this filtering, including three-axis accelerometer and three-axis gyroscope data, Indicates the IMU result after the last filtering, Indicates the original sampling value of IMU, represents the dynamic filtering factor, and n represents the number of iterations.

4. The combined navigation and positioning method for unmanned agricultural machinery according to claim 3, characterized in that: The dynamic filtering factor for: （2） in, is the velocity increment factor, and They are the accelerometer X-axis velocity increment output and Y-axis velocity increment output, is the IMU epoch, is the filtering window time, is the time constant of the first-order filter, represents the cutoff frequency, and are the accelerometer filter factor and the gyroscope filter factor, 、 and All are set by experience.

5. The combined navigation and positioning method for unmanned agricultural machinery according to claim 1, characterized in that: The step S4 comprises: According to the linearized extended Kalman filter measurement update equation, the Kalman filter is used to update the state quantity and covariance of the integrated navigation system. At the same time, the influence of abnormal interference in the real-time integrated navigation system is taken into account, and a gross error detection and fault tolerance mechanism is established. The current state of the integrated navigation system is judged based on the measurement innovation, and the normalized innovation vector is calculated.

6. The combined navigation and positioning method for unmanned agricultural machinery according to claim 5, characterized in that: The step S4 further includes: According to the value of the normalized innovation vector, the measurement innovation variance is adaptively adjusted; and the Kalman filter measurement update process is performed according to the adjusted measurement innovation variance.

7. A combined navigation and positioning system for unmanned agricultural machinery, characterized in that: Includes the following modules: The acquisition module collects GNSS positioning result data and IMU raw data, and uses a low-pass filter suitable for agricultural machinery movement scenarios to perform data filtering preprocessing on the IMU raw data. GNSS stands for Global Navigation Satellite System, and IMU stands for Micro Electro Mechanical System Inertial Sensor. The dead reckoning and filtering module initializes the integrated navigation system state based on the GNSS positioning results. The three-dimensional position and velocity of the GNSS positioning results are used as the initial position and velocity, and the three-dimensional position standard deviation is used as the judgment condition. When it is less than the threshold, the integrated navigation system state is initialized and INS inertial dead reckoning and Kalman filtering are performed in the initialized state. INS stands for inertial navigation system. The adjustment module determines the stationary or moving state of the agricultural machinery based on the results of the INS inertial navigation. It performs zero-speed correction when the machinery is stationary and applies adaptive non-holonomic constraints when the machinery is in motion. In addition, it uses an adaptive sliding window setting method to dynamically adjust the adaptive non-holonomic constraint noise for scenarios where the machinery is in uniform motion. According to the results of INS inertial navigation and IMU raw data, the motion state of agricultural machinery is judged and the motion discrimination parameters are set; Set the zero-speed detection threshold. If the motion discrimination parameter is less than the zero-speed detection threshold, it is considered to be in a stationary state and the zero-speed correction process is performed; If the motion discrimination parameter is greater than or equal to the zero-speed detection threshold, it is considered to be in motion, and adaptive non-holonomic constraint is performed. The adaptive non-holonomic constraint noise is adaptively adjusted according to the movement speed and steering angle of the agricultural machinery to obtain the steering angle change rate of the agricultural machinery. : （14） in, is the estimated heading angle at the current moment, is the IMU epoch, N is the total amount of data in the window time, The window time for obtaining the heading angle is adjusted adaptively according to the carrier's motion speed. and Set speed and time thresholds respectively; The estimation module uses the linearized extended Kalman filter measurement update equation to optimally estimate the state quantity and covariance of the integrated navigation system and obtain the system state error correction value; The output module feeds back the system state error correction value to the integrated navigation system, corrects the inertial navigation error, and outputs the integrated navigation positioning result in real time.

8. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the steps of the combined navigation and positioning method for unmanned agricultural machinery according to any one of claims 1 to 6 are implemented.

9. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the combined navigation and positioning method for unmanned agricultural machinery according to any one of claims 1 to 6 are implemented.

Images