Integrated navigation positioning method and system for unmanned agricultural machine

Abstract

The invention provides an integrated navigation and positioning method and system for unmanned agricultural machinery, and relates to the technical field of satellite navigation and positioning, and the method comprises the steps: S1, receiving and processing sensor data; s2, setting and stabilizing the initial state of the integrated navigation system; s3, motion state recognition and constraint are carried out; step S4, navigation state fault-tolerant filtering estimation; and S5, performing closed-loop feedback on the navigation state and outputting a navigation result. According to the invention, the fault-tolerant capability and stability of the integrated navigation system are obviously enhanced, and a continuous, stable and high-precision positioning result is provided for the unmanned agricultural machine.

Description

Technical Field

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

[0002] With the advancement of agricultural modernization, the demand for autonomous production operations of unmanned agricultural machinery is increasing day by day, and its core lies in obtaining stable, continuous, and high-precision positioning data support. Currently, although the Global Navigation Satellite System (GNSS) can provide accurate three-dimensional coordinates and speed information all-weather, it is vulnerable to environmental interference and has a low data update frequency; while the Inertial Navigation System (INS) has strong autonomy and outstanding anti-interference ability, but there is a problem of error accumulation. Therefore, the GNSS / INS integrated navigation system has emerged. By integrating the advantages of both, it has significantly improved the dynamic navigation and positioning performance in complex environments and achieved a more accurate and continuous reliable navigation service. With the development of Micro-Electro-Mechanical System (MEMS) technology, MEMS inertial sensors (MEMS-IMU) have become an ideal choice for GNSS / INS integrated navigation systems in agricultural scenarios due to their small size and low cost, showing broad application prospects.

[0003] However, in actual agricultural operation scenarios, the GNSS / INS integrated navigation system faces many challenges. On the one hand, the vibration and complex motion state of the agricultural machinery operation platform are likely to cause unstable zero-bias estimation of low-cost IMUs, resulting in rapid divergence of INS navigation results, and further reducing the accuracy of integrated navigation; on the other hand, the complexity of the operation environment and hardware failures may lead to the occurrence of errors and incorrect measurements, affecting the accuracy and reliability of the filtering results. Existing GNSS / INS integrated navigation technologies mostly use Extended Kalman Filter (EKF) for information fusion, but they fail to fully solve the above problems, resulting in limited application in unmanned agricultural machinery operations and being difficult to meet the requirements of unmanned agricultural machinery for continuous, stable, and high-precision navigation and positioning. Summary of the Invention

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

[0005] To achieve the above object, the present invention adopts the following technical solutions:

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

[0007] Step S1: Collect GNSS positioning result data and IMU raw data, and perform data filtering preprocessing on the IMU raw data using a low-pass filter applicable to the motion scenario of agricultural machinery; GNSS represents the Global Navigation Satellite System, and IMU represents the Micro-Electro-Mechanical System inertial sensor;

[0008] Step S2: Initialize the combined 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, perform the initialization of the combined navigation system state, and perform INS inertial navigation calculation and Kalman filtering with the initialized state; INS represents the Inertial Navigation System;

[0009] Step S3: Judge whether the agricultural machinery is in a stationary state or a moving state according to the result of the INS inertial navigation calculation. Perform zero-velocity correction when in a stationary state, and perform adaptive non-holonomic constraints when in a moving state. For the uniform motion scenario of agricultural machinery, adopt an adaptive sliding window setting method to dynamically adjust the adaptive non-holonomic constraint noise;

[0010] Step S4: Use the linearized extended Kalman filter measurement update equation to perform optimal estimation on the combined navigation system state quantity and its covariance to obtain the system state error correction value;

[0011] Step S5: Feed back the system state error correction value in Step S4 into the combined navigation system, correct the inertial navigation error, and output the combined navigation and 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] A data acquisition module that acquires GNSS positioning result data and IMU raw data, and performs data filtering preprocessing on the IMU raw data using a low-pass filter applicable to the motion scenario of agricultural machinery; GNSS represents the Global Navigation Satellite System, and IMU represents the Micro-Electro-Mechanical System inertial sensor;

[0014] An inference and filtering module that initializes the combined navigation system state based on the GNSS positioning result, uses the three-dimensional position and velocity of the GNSS positioning result as the initial position and velocity, and uses the three-dimensional position standard deviation as the judgment condition. When it is less than the threshold, the combined navigation system state is initialized, and INS inertial navigation inference and Kalman filtering are performed with the initialized state; INS represents the Inertial Navigation System;

[0015] An adjustment module that judges the stationary or moving state of the agricultural machinery according to the result of INS inertial navigation inference, performs zero-velocity correction in the stationary state, performs adaptive non-holonomic constraints in the moving state, and for the uniform motion scenario of the agricultural machinery, uses an adaptive sliding window setting method to dynamically adjust the adaptive non-holonomic constraint noise;

[0016] An estimation module that uses the linearized extended Kalman filter measurement update equation to perform optimal estimation on the combined navigation system state variables and their covariance to obtain the system state error correction value;

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

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

[0019] The present invention also provides a non-transitory computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the above-mentioned combined navigation positioning method for unmanned agricultural machinery are implemented.

[0020] Beneficial effects:

[0021] The present invention comprehensively considers the application scenario characteristics of unmanned agricultural machinery. By using a low-pass data filtering method suitable for the agricultural machinery scenario, it reduces the influence of vehicle body vibration on the high-frequency noise of inertial navigation observation data, takes into account the motion state of the agricultural machinery to constrain the INS mechanical arrangement result, and adopts an adaptive sliding window setting method during the movement of the agricultural machinery to flexibly adjust the NHC noise based on the motion speed and the heading change obtained according to the motion state of the agricultural machinery. Finally, by adding a fault detection and fault tolerance mechanism to the filtering estimation, it further weakens the influence of combined system errors and abnormal disturbances on state estimation, enhances the fault tolerance and stability of the integrated navigation method, and can provide continuous, stable, and high-precision positioning results for unmanned agricultural machinery. Brief Description of the Drawings

[0022] Figure 1 is a flowchart of the integrated navigation and positioning method for unmanned agricultural machinery according to the present invention.

[0023] Figure 2a , Figure 2b is a comparison diagram of the integrated navigation result trajectory; among them, Figure 2a is the integrated navigation and positioning result with only fixed NHC noise constraint, Figure 2b is the integrated navigation and positioning result under the present invention, where N represents north and E represents east.

[0024] Figure 3 is a schematic diagram of the integrated navigation and positioning device for unmanned agricultural machinery according to the present invention. Detailed Embodiment

[0025] In order to make the objectives, technical solutions, and advantages of the present invention clearer, the following further details the present invention in combination with the 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 the vibration of the agricultural machinery carrier and reduces the influence of the vibration on the original observation data of the low-cost inertial sensor through data filtering means; in the process of information fusion, the integrated navigation performance is further enhanced by adding carrier motion state constraints, including zero velocity correction and non-integrability constraints, and considering the vehicle turning state, an NHC adaptive factor is introduced to reduce the influence of lateral velocity noise on the non-integrability constraint; finally, in the filtering estimation, considering the combined system error and abnormal interference, etc., a gross error detection and fault tolerance mechanism is adopted to enhance the fault tolerance and stability of the filter.

[0026] As Figure 1 shown, the present invention provides an integrated navigation and positioning method for unmanned agricultural machinery, including the following steps:

[0027] Step S1. Sensor data reception and processing, including:

[0028] Collect, decode, and preprocess the real-time data stream of the sensor, including the three-dimensional position, velocity of GNSS, and the raw data of IMU. Only decode and store the data of the three-dimensional position and velocity of GNSS, and record its position, velocity, corresponding covariance, and its positioning status information; decode, store, and preprocess the raw data of IMU, including the data of the three-axis accelerometer and the three-axis gyroscope. To reduce the impact of the vibration of the agricultural machinery carrier on the IMU data, further preprocess the acceleration and angular velocity data by data filtering, design a low-pass filter suitable for the motion scenario of agricultural machinery, and select a low-pass filtering cut-off frequency lower than the vibration frequency of agricultural machinery. The formula is as follows:

[0029] (1)

[0030] Where, Represents the IMU result after this filtering, including the data of the three-axis accelerometer and the three-axis gyroscope, Represents the IMU result after the previous filtering, Represents the raw sampling value of IMU, Represents the dynamic filtering factor, and n represents the number of iterations. Since the sensitivities of the three-axis gyroscope and the three-axis accelerometer in the IMU sensor to the vibration of agricultural machinery are different, and for the motion of agricultural machinery at different speeds, the impact of the vibration state of agricultural machinery on the IMU data is also different, the dynamic filtering factor needs to be set as follows:

[0031] (2)

[0032] Where, Is the speed increment factor, And Are the speed increment outputs of the X-axis and Y-axis of the accelerometer respectively, Is the IMU epoch, Is the filtering window time, Is the time coefficient of the first-order filter, Represents the cut-off frequency, And Are the filtering factors of the accelerometer and the gyroscope, , And Are all set by empirical values.

[0033] Step S2. Initial state setting and steady-stateization of the integrated navigation system, including:

[0034] Initialize the combined navigation system state based on the GNSS positioning result. Use the real-time received GNSS three-dimensional position and velocity as the initial position and velocity of the combined navigation system. At the same time, to ensure the reliability of the initial state, use the standard deviation of the GNSS three-dimensional position result as the judgment condition. If it is less than the set standard deviation threshold , then the initialization process of the combined navigation system state can be carried out. Initialize the combined navigation system with the GNSS positioning result at this moment as the initial value, and use this 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 recognition and constraint, including:

[0036] Step S3.1 Judge the motion state of the agricultural machinery according to the result of INS inertial navigation calculation and the original IMU data, and set the motion discrimination parameter , and its calculation formula is:

[0037] (3)

[0038] Among them, m represents the number of IMU epoch sliding windows used for calculating , represents the IMU epoch, represents the gravitational acceleration of the current epoch carrier in the navigation coordinate system ( system), ; x, y, z represent the three coordinate axes of the system, represents the specific force measurement value of the accelerometer sensor at the current epoch,

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

[0040] Step S3.2 When it is determined that the agricultural machinery is in a stationary state, perform the static zero-speed update (ZUPT) process. It is considered that the velocity in the current body coordinate system ( system) is 0, and the difference in the heading angle from the previous position is 0, that is:

[0041] (4)

[0042] Among them, is the speed of the download carrier, , and are respectively the speeds of the download carrier in the front, right, and lower directions. According to the above constraint conditions, that is, formula (4), the speed constraint observation equation in the stationary state can be constructed:

[0043] (5)

[0044] Among them, is the observation innovation vector of ZUPT, is the carrier speed in the system calculated by inertial navigation. Due to the existence of inertial navigation errors, its value is generally not 0. is the state parameter of the integrated navigation system, which is 21-dimensional. is the measurement noise. is the ZUPT measurement matrix, and there is:

[0045] (6)

[0046] Among them, represents the 3×3 unit vector, and represent the 3×3 and 3×15 zero vectors respectively;

[0047] Step S3.3 If the agricultural machinery is in a moving state, then an adaptive non-integrity constraint process is performed. It is considered that when the agricultural machinery is moving, its system forward speed is the carrier movement speed, while the speeds in the lateral and vertical directions are zero, that is:

[0048] (7)

[0049] Among them, is the speed of the download carrier, , and are respectively the speeds of the download carrier in the front, right, and lower directions. Similarly, the obtained from the inertial navigation mechanical arrangement can be used to calculate the actual speed of the download carrier:

[0050] (8)

[0051] Among them, is the speed of the download carrier, is the attitude transformation matrix between the systems, and the superscript T represents the transpose of the matrix.

[0052] Performing error perturbation analysis on the above formula and subtracting the actual calculation speed from the theoretical true speed, we have:

[0053] (9)

[0054] where and are the velocity error state quantity and attitude error state quantity in the n system, respectively. is the symbol of the skew-symmetric matrix. According to the above formula, the velocity constraint observation equation can be constructed. Among them, NHC only targets the lateral velocity and vertical velocity. Therefore, only the lateral component and vertical component are taken in this observation equation:

[0055] (10)

[0056] (11)

[0057] where is the NHC velocity noise, and represent the observation information vector and measurement matrix constrained by NHC, respectively. is the state parameter of the integrated navigation system. represents the 1st to 3rd columns of the 2nd and 3rd rows of and represent zero vectors of 2 rows and 3 columns and 2 rows and 12 columns, respectively.

[0058] For agricultural machinery operating in the field, it is inevitable to have more or less left-right shaking due to the special environment of the farmland. Moreover, due to factors such as the turning of agricultural machinery and road bumps, certain lateral velocity noise and vertical velocity noise will be generated. Therefore, the NHC noise cannot be simply set to a fixed value and needs to be adaptively adjusted according to the motion state of the agricultural machinery. Here, the NHC noise is adaptively adjusted according to the motion speed and steering angle of the agricultural machinery:

[0059] (12)

[0060] (13)

[0061] where represents the steering angle of the agricultural machinery, is the NHC velocity noise matrix, and are the lateral velocity noise and longitudinal velocity noise, respectively. is the course angle change value, , are the set lateral adaptive scale factor and longitudinal adaptive scale factor;

[0062] The steering angle of the agricultural machinery is determined according to the degree of change of the course angle within a certain window time:

[0063] (14)

[0064] (15)

[0065] Among them, is the estimated value of the course angle at the current moment, is the IMU epoch, N is the total amount of data in the window time, is the window time for calculating the course angle, and the window time for calculating the course angle is adaptively adjusted according to the carrier motion speed, and are the set speed and time threshold respectively. In addition, the window time for calculating the course angle sets the lowest threshold to avoid abnormal calculation caused by too high speed of the agricultural machinery;

[0066] According to the above motion constraint equations, namely formula (4) and formula (7), after each INS inertial navigation calculation, the motion state is judged and corresponding constraints are carried out to obtain the constrained error state quantity and its covariance, and feedback them to the pose result of the mechanical arrangement.

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

[0068] Step S4.1 According to the linearized extended Kalman filter measurement update equation, the state quantity and its covariance of the integrated navigation system are updated by Kalman filter. At the same time, considering the influence of abnormal interference existing in the real-time integrated navigation system, a gross error detection and fault-tolerant mechanism is established, and the state of the current integrated navigation system is judged according to the measurement innovation. Among them, the measurement innovation is the difference between the actual observation value and the predicted value of the observed quantity:

[0069] (16)

[0070] The calculation formula of the measurement innovation variance matrix is as follows:

[0071] (17)

[0072] Among them, is the loose integrated measurement update observation value, is the measurement update matrix of the loose integration, is the combined system state vector, is the normalized innovation vector, which can reflect the matching degree between the actual value and the theoretical value of the observed innovation. represents the measurement innovation value corresponding to the j-th observable, represents the variance value corresponding to the j-th observable, is the observable noise covariance matrix, is the prediction error covariance matrix at time k;

[0073] Step S4.2 According to the normalized innovation vector value, perform adaptive adjustment of the measurement innovation variance, so as to control the influence of the measurement innovation in the measurement update, and reduce the influence of the combined navigation system error and abnormal interference on the state estimation; set the threshold , when , it is considered that a fault has occurred, and the adjusted measurement innovation variance is obtained:

[0074] (18)

[0075] Step S4.3 According to the adjusted measurement innovation variance perform the Kalman filter measurement update process.

[0076] Step S5, closed-loop feedback of the navigation state and output of the navigation result, including:

[0077] Feed the system state parameters back to the combined navigation system to realize the closed-loop feedback of the combined navigation error. On the one hand, it is the feedback of the position, speed, and attitude errors, and the corresponding state errors are corrected to the INS calculation result and the result is output. On the other hand, it is the feedback of the inertial sensor errors (accelerometer and gyro zero biases), and the output value of the inertial sensor is corrected using its error estimation, and subsequent INS calculations are performed based on the corrected inertial sensor data.

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

[0079] Acquisition module, which acquires GNSS positioning result data and IMU raw data, and performs data filtering preprocessing on the IMU raw data using a low-pass filter suitable for the agricultural machinery movement scenario;

[0080] The calculation and filtering module initializes the combined navigation system state based on the GNSS positioning result, uses the three-dimensional position and velocity of the GNSS positioning result as the initial position and velocity, and takes the three-dimensional position standard deviation as the judgment condition. When it is less than the threshold, it initializes the combined navigation system state and performs INS inertial navigation calculation and Kalman filtering with the initialized state;

[0081] The adjustment module determines the stationary or moving state of the agricultural machinery according to the result of INS inertial navigation calculation. When in the stationary state, zero-speed correction is performed, and when in the moving state, adaptive nonholonomic constraints are performed. For the scenario of the agricultural machinery moving at a constant speed, an adaptive sliding window setting method is used to dynamically adjust the adaptive nonholonomic constraint noise;

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

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

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

[0085] The present invention also provides a non-transitory computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the above-mentioned combined navigation positioning method for unmanned agricultural machinery are implemented.

[0086] Embodiment:

[0087] Using a set of actually collected low-cost IMU and GNSS result data for real-time combined navigation simulation testing in the agricultural machinery scenario, according to the vehicle body vibration frequency of the agricultural machinery in actual movement, simulating the original data output in the agricultural machinery operation scenario, and verifying the effectiveness of the present invention. The parameter settings involved in the specific processing process are as follows:

[0088] Step 1: Obtain the GNSS positioning result and the original IMU data, and preprocess the IMU data. The dynamic filtering factor calculation window time is set to 5 s, and the low-pass filtering cut-off frequency is 15 Hz;

[0089] Step 2: According to the empirical value, set the position standard deviation threshold to 1.0 m. When the GNSS three-dimensional position standard deviation , initialize the state of the integrated navigation system, and use the epoch position, velocity, and attitude results as the initial values for INS inertial navigation calculation and Kalman filter update;

[0090] In step 3, motion state recognition and constraint, for low-speed motion of agricultural machinery, set the number of IMU epoch sliding windows for motion judgment discriminant 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 to 1.5, to 1.0, and in the calculation of the window value for the adaptive change value of the heading angle and the thresholds are set to 2m / s and 2s respectively, and perform motion state recognition and corresponding motion constraints with the above parameter settings;

[0091] In step 4, in the navigation state fault-tolerant filtering estimation, set the threshold of the normalized innovation vector to 3.0;

[0092] In step 5, based on the above parameter settings, finally realize the closed-loop feedback of the navigation state and the output of the navigation result.

[0093] Compare the integrated navigation positioning result under the present invention with the integrated navigation positioning result that only uses fixed NHC noise constraint. The corresponding integrated navigation positioning trajectory diagram is as Figure 2a , Figure 2b shown, where Figure 2a is the integrated navigation positioning result that only uses fixed NHC noise constraint, Figure 2b is the integrated navigation positioning result under the present invention. Convert the positioning result to the northeast celestial coordinate system, and its horizontal and vertical coordinates represent the coordinate values in the east and north directions respectively. From Figure 2a , Figure 2b it can be seen that the integrated navigation result under the present invention is smoother. The low-pass filter weakens the influence of vehicle body vibration on the IMU. In addition, for the positioning result in the turning interval, the adaptive NHC noise determination adopted by the present invention further enhances the navigation positioning accuracy in this interval.

[0094] Those skilled in the art should understand that the embodiments of the present invention can be provided as a method, a system, or a computer program product. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk memory, CD-ROM, optical memory, etc.) that contain computer-usable program code. The solutions in the embodiments of the present invention can be implemented in various computer languages. For example, object-oriented programming languages such as Java and interpreted scripting languages such as JavaScript, etc.

[0095] The present invention is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to embodiments of the present invention. It should be understood that each flow and / or block in the flowchart and / or block diagram, as well as the combination of flows and / or blocks in the flowchart and / or block diagram, can be realized by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing devices generate means for implementing the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0096] These computer program instructions can 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, such that the instructions stored in the computer-readable memory generate a manufactured article including instruction means that implement the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0097] These computer program instructions can also be loaded onto a computer or other programmable data processing device, such that a series of operation steps are executed on the computer or other programmable device to generate a computer-implemented process. Thus, the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0098] Although the preferred embodiments of the present invention have been described, those skilled in the art can make additional changes and modifications once they learn the basic creative concepts. Therefore, the appended claims are intended to be construed 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 can make various modifications and variations to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalent technologies, the present invention is also intended to include these modifications and variations.

Claims

1. A combined navigation and positioning method for unmanned agricultural machinery, characterized in that, It includes the following steps: Step S1: Collect GNSS positioning result data and IMU raw data, and perform data filtering preprocessing on the IMU raw data using a low-pass filter applicable to the agricultural machinery movement scenario; GNSS represents the Global Navigation Satellite System, and IMU represents the 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, perform the integrated navigation system state initialization, and perform INS inertial navigation calculation and Kalman filtering with the initialized state; INS represents the Inertial Navigation System; Step S3: Judge whether the agricultural machinery is in a stationary state or a moving state according to the result of INS inertial navigation calculation. Perform zero-velocity correction in the stationary state, and perform adaptive non-holonomic constraints in the moving state. For the scenario of uniform movement of agricultural machinery, use the adaptive sliding window setting method to dynamically adjust the adaptive non-holonomic constraint noise; Step S4: Use the linearized extended Kalman filter measurement update equation to perform optimal estimation on the integrated navigation system state quantity and its covariance to obtain the system state error correction value; Step S5: Feed back the system state error correction value in Step S4 into 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 said Step S1 includes: Decode and store the data of the GNSS positioning result, and record its position, velocity, covariance and positioning status information; decode, store and preprocess the IMU raw data, including performing low-pass filtering on the three-axis accelerometer data and 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 applicable to the agricultural machinery movement scenario in the said Step S1 is: （1） Among them, represents the IMU result after this filtering, including triaxial accelerometer and triaxial gyroscope data, represents the IMU result after the previous filtering, represents the original IMU sampled value, 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 is as follows: （2） Wherein, is the speed increment factor, and are the speed increment outputs of the accelerometer on the X-axis and the Y-axis respectively, is the IMU epoch, is the filtering window time, is the time coefficient of the first-order filter, represents the cut-off frequency, and are the accelerometer filtering factor and the gyroscope filtering factor, , and are all set by empirical values.

5. The combined navigation and positioning method for unmanned agricultural machinery according to claim 1, characterized in that, The said Step S3 includes: Judging the motion state of agricultural machinery according to the results calculated by INS inertial navigation and the original IMU data, and setting motion discrimination parameters; setting a 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 a zero-speed correction process is carried out; if the motion discrimination parameter is greater than or equal to the zero-speed detection threshold, it is considered to be in a motion state and an adaptive nonholonomic constraint is carried out. The adaptive nonholonomic constraint noise is adaptively adjusted according to the motion speed and steering angle of the agricultural machinery to obtain the steering angle of the agricultural machinery : （14） Among them, is the estimated value of the heading angle at the current moment, is the IMU epoch, and N is the total amount of data in the window time. is the window time for obtaining the heading angle, and the window time for obtaining the heading angle is adaptively adjusted according to the carrier motion speed. and are the set speed and time threshold respectively.

6. The combined navigation and positioning method for unmanned agricultural machinery according to claim 1, characterized in that, The said Step S4 includes: According to the linearized extended Kalman filter measurement update equation, perform Kalman filter update on the integrated navigation system state quantity and its covariance, and at the same time take into account the abnormal interference effects existing in the real-time integrated navigation system, establish a gross error detection and fault tolerance mechanism, judge the state of the current integrated navigation system according to the measurement innovation, and calculate the normalized innovation vector.

7. The combined navigation and positioning method for unmanned agricultural machinery according to claim 6, characterized in that, The said Step S4 further includes: Perform adaptive adjustment of the measurement innovation variance according to the value of the normalized innovation vector; perform the Kalman filter measurement update process according to the adjusted measurement innovation variance.

8. A combined navigation and positioning system for unmanned agricultural machinery, characterized in that, It includes the following modules: Acquisition module, which collects GNSS positioning result data and IMU raw data, and performs data filtering preprocessing on the IMU raw data using a low-pass filter applicable to the agricultural machinery movement scenario; GNSS represents the Global Navigation Satellite System, and IMU represents the Micro-Electro-Mechanical System inertial sensor; Calculation and filtering module, which initializes the integrated navigation system state according to the GNSS positioning result, uses the three-dimensional position and velocity of the GNSS positioning result as the initial position and velocity, and uses the three-dimensional position standard deviation as the judgment condition. When it is less than the threshold, perform the integrated navigation system state initialization, and perform INS inertial navigation calculation and Kalman filtering with the initialized state; INS represents the Inertial Navigation System; The adjustment module determines the stationary or moving state of the agricultural machinery according to the results calculated by the INS inertial navigation. Zero-speed correction is performed in the stationary state, and adaptive nonholonomic constraints are performed in the moving state. For the uniform motion scenario of the agricultural machinery, an adaptive sliding window setting method is used to dynamically adjust the adaptive nonholonomic constraint noise; The estimation module uses the measurement update equation of the linearized extended Kalman filter to optimally estimate the state variables and their covariance of the integrated navigation system, and obtains the system state error correction value; The output module feeds the system state error correction value back into the integrated navigation system, corrects the inertial navigation error, and outputs the integrated navigation positioning result in real time.

9. An electronic device, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, When the processor executes the program, the steps of the integrated navigation positioning method for unmanned agricultural machinery according to any one of claims 1 to 7 are implemented.

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

Images