An inertial heading constraint method and device based on a farmland operation scene

Abstract

The application provides an inertial navigation constraint method and device based on a farmland operation scene, and belongs to the technical field of satellite navigation and positioning, and comprises the following steps: identifying a farmland operation state in real time, determining that a farmland machine is currently in a straight-line operation state, a stationary state or a turning state according to GNSS position, IMU angular velocity and IMU acceleration, and giving an operation state factor; when and only when it is determined that the farmland machine is in the straight-line operation state, extracting a path reference heading of a preset path from a preset path planning as a virtual heading observation value; synchronously sending the virtual heading observation value and the GNSS position, GNSS speed, IMU angular velocity and IMU acceleration into an extended Kalman filter, wherein an extended state vector of the extended Kalman filter contains the operation state factor, the heading observation noise weight is dynamically adjusted according to the operation state factor, and the position, speed and attitude information of the farmland machine after correction is output. The application realizes stable and more accurate navigation and positioning.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of satellite navigation and positioning, and particularly relates to an inertial heading constraint method and device based on a farmland operation scene. BACKGROUND

[0002] With the rapid development of agricultural automation and intelligence, the application of unmanned agricultural machines is gradually popularizing. These unmanned agricultural machines usually need to operate autonomously in complex and variable farmland environments to perform tasks such as seeding, fertilizing, spraying, etc. The successful performance of these tasks largely depends on accurate and reliable positioning information. Global Navigation Satellite System (GNSS) as a global positioning technology provides position, velocity and time information for unmanned agricultural machines. However, the position information provided by GNSS has a low update frequency, which cannot meet the control requirements of agricultural machines, and the GNSS signal may be interfered or shielded by buildings, trees or hills, etc., thereby affecting the positioning accuracy. Therefore, an inertial navigation system (INS) is usually combined with GNSS to make up for the deficiency of GNSS. GNSS / INS (inertial navigation system) integrated navigation technology combines the advantages of GNSS and INS, and uses filtering algorithms (such as Kalman filtering) to fuse the data of the two to obtain more stable and accurate positioning results. Under this technology, GNSS provides long-term position information, and INS provides short-term high-precision position information, and through combination, continuous and reliable positioning is obtained.

[0003] The main automatic operations of agricultural machines include seeding, fertilizing, spraying, deep ploughing, target, ridging, rotary tillage, etc., which have some common characteristics, and the main paths are in the form of straight line + turning + straight line + turning. In the traditional GNSS / INS integrated navigation, when in continuous straight line motion, there is no observable in the heading of the system, which will cause the heading accuracy to diverge with time, and this divergence is more fatal to low-grade precision IMU (inertial measurement unit), which will directly lead to unreliable positioning accuracy, thereby causing deviation in the path tracking of agricultural machines, seriously affecting the quality of field operation. At present, the methods for suppressing heading divergence mainly include double-antenna heading constraint and extrapolation of position, velocity and attitude according to the current motion state and then combined constraint, the former method has stable effect but high cost, and the latter method has complex motion model construction and untrustworthy extrapolation result. SUMMARY

[0004] In view of the problem that the heading accuracy of GNSS / INS integrated navigation technology in the continuous straight-line operation of unmanned agricultural vehicles in automatic driving is divergent with time, resulting in the decline of positioning accuracy, the application provides an inertial heading constraint method and device based on farmland operation scenarios, which fully combines path planning information and agricultural operation state, and proposes an innovative fusion constraint mechanism of "virtual heading observation" and "operation state factor". The application introduces a new type of state variable "operation state factor" s∈{in-field straight-line operation, static, non-operation turning}, dynamically describes the operation mode of the agricultural vehicle, cooperates with the GNSS / INS combined Kalman filter system, applies virtual heading observation constraint when it is identified as "straight-line state", extracts the reliable heading from the path planning , corrects in the form of soft constraint in the filter observation equation, and realizes stable and more accurate navigation positioning.

[0005] In order to achieve the above purpose, the application adopts the following technical scheme:

[0006] An inertial heading constraint method based on farmland operation scenarios, comprising the following steps:

[0007] S1, real-time identification of the operation state of the agricultural vehicle, judgment of the current straight-line operation, static or turning state of the agricultural vehicle according to the GNSS position, IMU angular velocity and IMU acceleration, and assignment of different operation state factors;

[0008] S2, when and only when the agricultural vehicle is determined to be in the straight-line operation state, the path reference heading of the preset path is extracted from the preset path planning as the virtual heading observation value;

[0009] S3, the virtual heading observation value, GNSS position, GNSS speed, IMU angular velocity and IMU acceleration are synchronously sent into an extended Kalman filter, the extended state vector of the extended Kalman filter contains the operation state factor, the heading observation noise weight is dynamically adjusted according to the operation state factor, and the corrected position, speed and attitude of the agricultural vehicle are output.

[0010] The application also provides an inertial heading constraint device based on farmland operation scenarios, which comprises a GNSS antenna, a GNSS solution module, an IMU module, a path planning information module, a core processor module and an output module; the IMU represents an inertial measurement unit.

[0011] The GNSS antenna provides GNSS radio frequency signals to the GNSS solution module, and after being solved by the GNSS solution module, GNSS position information and GNSS speed information are transmitted to the core processor module; the IMU module transmits three-dimensional acceleration information and three-dimensional angular velocity information of the IMU to the core processor module; before operation, the automatic operation path of the agricultural machine is input to the path planning information module, and after being processed by the path planning information module, position information and path heading information of the path point are provided to the core processor module; the core processor module combines and solves the received information to obtain the position, speed and attitude of the agricultural machine, and transmits them to the output module.

[0012] The application further provides an electronic device, including a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the inertial navigation constraint method based on the agricultural field operation scene when executing the program.

[0013] The application further provides a non-transitory computer readable storage medium, which stores a computer program, wherein the computer program is executed by a processor to implement the steps of the inertial navigation constraint method based on the agricultural field operation scene.

[0014] Advantages:

[0015] 1) The application introduces an operation state factor as part of the navigation system state, dynamically identifies the operation state of the agricultural machine in the integrated navigation, and actively adjusts the heading constraint strength;

[0016] 2) The application uses the heading information in path planning to construct a virtual heading observation, provides additional heading correction information in the straight-line operation state, and effectively suppresses the heading divergence problem;

[0017] 3) The application does not rely on additional hardware, and can be realized only by using the existing GNSS and IMU information, and has the advantages of simple engineering implementation, low cost and strong adaptability;

[0018] 4) The application enhances the navigation stability of the agricultural machine in continuous straight-line and turning scenarios, improves the path tracking accuracy, and is especially suitable for high-precision unmanned operation navigation systems in fields.

[0019] In summary, the "operation state perception + virtual heading observation" integrated navigation method proposed by the application constructs a heading soft constraint strategy for the agricultural field operation scene, improves the tracking accuracy of the agricultural machine automatic operation without introducing additional physical observation conditions, and has significant practical and promotional value. BRIEF DESCRIPTION OF DRAWINGS

[0020] Figure 1 is a schematic diagram of an inertial navigation constraint device based on an agricultural field operation scene of the application;

[0021] Figure 2 A flow chart of an inertial navigation constraint method based on a farmland operation scenario of the present application;

[0022] Figure 3 A schematic diagram for planning an operation path;

[0023] Figure 4 A schematic diagram of actual tracking results, in which blue is an unconstrained tracking trajectory and orange is a tracking trajectory after the application of the constraint of the present application. DETAILED DESCRIPTION

[0024] In order to make the objectives, technical solutions and advantages of the present application clearer, the present application 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 application and do not limit the present application. In addition, the technical features involved in the various embodiments of the present application described below can be combined with each other as long as they do not conflict with each other.

[0025] As shown in Figure 1 , the present application provides an inertial navigation constraint device based on a farmland operation scenario, comprising a GNSS antenna, a GNSS solution module, an IMU (inertial measurement unit) module, a path planning information module, a core processor module, and an output module.

[0026] The GNSS antenna provides GNSS radio frequency signals to the GNSS solution module, and transmits GNSS position and GNSS speed information to the core processor module after being solved by the GNSS solution module. The IMU module transmits three-dimensional acceleration information and three-dimensional angular velocity information of the IMU to the core processor module. Before operation, the automatic operation path of the agricultural machine is input to the path planning information module, and path point information and path heading information are provided to the core processor module after being processed by the path planning information module. The core processor module combines and solves the information provided by the above modules to obtain position, speed, and attitude information, and transmits the information to the output module. The output module can include a serial port, a network port, and a TCP.

[0027] As shown in Figure 2 , the present application provides an inertial navigation constraint method based on a farmland operation scenario, comprising the following steps:

[0028] S1: automatic operation condition recognition of an agricultural machine, comprising:

[0029] S1.1: obtaining IMU three-dimensional angular velocity ( ), three-dimensional acceleration a, GNSS position point position , and GNSS speed ; X, Y, Z are the three-axis direction angular velocities, respectively, X, Y, Z represent the three direction position coordinates, respectively, wherein X and Y are the plane, and Z is the elevation, X, Y, Z represent the three direction velocity components, respectively.

[0030] S1.2: Solve the curvature of the line connected by the sliding window point set, including:

[0031] Set the sliding window to 3, fit the plane coordinates of the 3 GNSS position points in the current sliding window , i = 1, 2, 3 represent three points, judge whether the line connected by the three point set in the sliding window meets the straight line feature stably, estimate the curvature using the three-point circle fitting method, that is, set the three points as: , , , calculate the side length of the triangle composed of the three points in the sliding window:

[0032] ;

[0033] ;

[0034] ;

[0035] The semi-perimeter of triangle is , the area of triangle is ; if A , is the area threshold, set to 0.01 , the line connected by the three points in the sliding window is a straight line, and the curvature k = 0 of the straight line; otherwise, the line connected by the three points in the sliding window is a curve, and a unique circle is determined according to the three points, and the curvature of the circle is .

[0036] S1.3: Determine the agricultural machinery operation state, including:

[0037] If ≤ , > , and the variance of the Z-axis angular velocity of the IMU is , it is determined that the agricultural machinery is in a straight line operation state, denoted as operation state factor s = 1, wherein is the curvature threshold, is the speed threshold, is the angular velocity variance threshold, indicates variance calculation; if If s = 0, it is determined that the agricultural machine is in a stationary state, and a work state factor s = 0 is recorded.

[0038] If k > 0, it is determined that the agricultural machine is in a straight-line work state, and a work state factor s = 1 is recorded. , If the variance of the IMU Z-axis angular velocity is greater than the variance of the IMU Y-axis angular velocity , it is determined that the agricultural machine is in a turning state, and a work state factor s = 2 is recorded.

[0039] S2: Extracting a virtual heading, including:

[0040] S2.1: According to the position of the GNSS position point , match the path planning segment, and extract the path reference heading from the path planning information of the path planning segment .

[0041] S2.2: Obtain the current actual heading of the agricultural machine from the integrated navigation system . If the GNSS provides valid speed observations, i.e., GNSS speed , calculate the current actual heading of the agricultural machine using the speed vector . If the INS is the main attitude solver, directly obtain the current actual heading of the agricultural machine through the solving result .

[0042] S2.3: Calculate the difference between the current actual heading of the agricultural machine and the path reference heading. If the difference between the two is: , it is determined that the path reference heading is a reliable heading, which can be used as a virtual heading observation value to be added to the filter for constraint, where is a heading determination threshold, is the difference between the actual motion heading and the path reference heading .

[0043] S3: Construct a GNSS / INS filter integrating virtual heading constraints, including:

[0044] S3.1: Extend the state vector , where is the position error vector, is the speed error vector, is the attitude error vector (Euler angle error), is the accelerometer bias, is the gyroscope bias, is the discrete work state factor, where s = 0 is the stationary state, s = 1 is the straight-line work state in the field, and s = 2 is the non-work turning state. The discrete work state factor can affect the observation weight adjustment in the filter. ​

[0045] S3.2: Constructing the Kalman filter state equation:

[0046] ;

[0047] in, For time t The estimated value, This represents the state transition matrix of the system at time t. Let represent the system noise matrix at time t. The driving matrix represents the system noise at time t.

[0048] S3.3: The Kalman filter observation equation is constructed as follows:

[0049] ;

[0050] in, For observation purposes, INS navigation location calculation results and the location of GNSS position points difference; Speed ​​results calculated for INS navigation and GNSS speed difference; The current actual heading of the agricultural machinery Path reference heading difference; The coefficient matrix, ,in This indicates that the heading angle (usually the Z-axis Euler angle) is selected as the observation direction; The observation noise matrix is ​​represented by the superscript T, which indicates the transpose of the matrix, and I represents the identity matrix.

[0051] S3.4: Set the observation noise matrix for:

[0052] ;

[0053] in, For GNSS positioning observation noise, For velocity observation noise, For dynamically adjusted heading virtual observation noise, where To minimize the virtual observation noise of the heading, Let t be the maximum virtual observation noise value for the heading, and t be time. It can adaptively adjust according to the status of automatic operation of agricultural machinery, reflecting a soft constraint strategy. diag() represents a diagonal matrix.

[0054] S3.5: complete the Kalman filter construction through S3.1, S3.2, S3.3 above, and substitute the Kalman filter state equation of S3.2 and the Kalman filter observation equation of S3.3 into the Kalman filter, so as to obtain the three-dimensional position, three-dimensional velocity and three-dimensional attitude of the agricultural machine.

[0055] Embodiment:

[0056] Curvature threshold is 0.015 , velocity threshold is 0.2 mm / s, angular velocity variance threshold is 0.003 rad / s, heading difference threshold is 0.1 rad, and virtual heading observation noise minimum value and maximum value are 0.5° and 10° respectively. Through the above settings, the planned operation path is as shown in Figure 3 , and the actual tracking result is as shown in Figure 4 , wherein the blue color represents the unconstrained tracking trajectory, and the orange color represents the tracking trajectory after the application of the application, and the trajectory is smoother and more continuous.

[0057] The application further provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, and the processor implements the steps of the inertial navigation constraint method based on the farmland operation scene when executing the program.

[0058] The application further provides a non-transitory computer readable storage medium, which stores a computer program, and the computer program implements the steps of the inertial navigation constraint method based on the farmland operation scene when executed by a processor.

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

Claims

1. A method for inertial heading constraint based on agricultural field work scenarios, the method comprising: Comprise the following steps: S1, real-time identification of agricultural machinery operation state, according to GNSS position, IMU angular velocity and IMU acceleration to judge the current agricultural machinery is in straight line operation, static or steering state, and give different operation state factor; The determination of operation state factor is realized according to the curvature of the fitting of the last three GNSS position points in the sliding window; If the curvature is less than or equal to the curvature threshold, the speed is greater than the speed threshold, and the Z-axis angular velocity variance is less than the angular velocity variance threshold, it is marked as straight line operation and the operation state factor is set to 1; If the speed is lower than the speed threshold, it is marked as static and the operation state factor is set to 0; If the curvature is greater than the curvature threshold and the angular velocity variance is greater than the angular velocity variance threshold, it is marked as steering and the operation state factor is set to 2; S2, only when the agricultural machinery is judged to be in straight line operation state, the path reference heading of the preset path is extracted from the preset path planning as the virtual heading observation value; S3, the virtual heading observation value and GNSS position, GNSS speed, IMU angular velocity, IMU acceleration are sent into the extended Kalman filter synchronously, the extended state vector of the extended Kalman filter contains the operation state factor, the heading observation noise weight is dynamically adjusted according to the operation state factor, and the corrected position, speed and attitude of the agricultural machinery are output.

2. The inertial heading constraint method based on farmland operation scenarios according to claim 1, characterized in that, If the area surrounded by the last three GNSS position points in the sliding window is less than the area threshold, it is determined that the last three GNSS position points in the sliding window are collinear, and the curvature is zero.

3. The method of claim 2, wherein, If the area surrounded by the last three GNSS position points in the sliding window is greater than or equal to the area threshold, the curvature is the curvature of the circumscribed circle of the last three GNSS position points in the sliding window.

4. The inertial heading constraint method based on farmland operation scenarios according to claim 3, characterized in that, If the difference between the actual motion heading and the path reference heading is less than the threshold, it is determined that the path reference heading is a reliable heading, which is added to the extended Kalman filter as a virtual heading observation value for constraint.

5. The inertial heading constraint method based on farmland operation scenarios according to claim 1, characterized in that, The heading observation noise is adaptively changed according to the operation state factor.

6. An inertial heading constraint device based on the agricultural field operation scenario according to any one of claims 1-5, characterized in that, It comprises GNSS antenna, GNSS solution module, IMU module, path planning information module, core processor module and output module; IMU represents inertial measurement unit; The GNSS antenna provides GNSS radio frequency signal to the GNSS solution module, and transmits GNSS position information and GNSS speed information to the core processor module after being solved by the GNSS solution module; The IMU module transmits three-dimensional acceleration information and three-dimensional angular velocity information of the IMU to the core processor module; Before operation, the automatic operation path of the agricultural machinery is input to the path planning information module, and the position information and path heading information of the path point are provided to the core processor module after being processed by the path planning information module; The core processor module combines and solves the received information to obtain the position, speed and attitude of the agricultural machinery, and transmits them to the output module.

7. An electronic device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor executes the program to realize the steps of the inertial heading constraint method based on the agricultural field operation scene as claimed in any one of claims 1 to 5.

8. A non-transitory computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to realize the steps of the inertial heading constraint method based on the agricultural field operation scene as claimed in any one of claims 1 to 5.

Images