Agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion

Abstract

The invention provides an agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion, and the algorithm carries out the fusion processing of the GNSS and IMU signals, and can make up the defect of the low updating frequency of the GNSS signal through the high-frequency data of the IMU. The method comprises the following steps: acquiring elevation data and acceleration data through a GNSS module and an I MU module which are respectively arranged on an agricultural machine head and an agricultural implement, processing the elevation data and the acceleration data through a Kalman filter to obtain high-precision elevation data of the elevation data and the acceleration data, and fusing position data acquired by a GNSS receiver and attitude data acquired by the I MU through Euler projection. And calculating the accurate coordinates of the gravity center points of the agricultural machine head and the agricultural implement. According to the two calculation results, high-precision terrain information is generated by using GNSS / IMU data of the machine head, the elevation variation of the machine head is used as a reference datum, the GNSS / IMU data of the farm implement is used as an input value of leveling profiling control as a reference of closed loop feedback, and profiling control of the farm implement is carried out on the suspension oil cylinder by using a PID algorithm. Therefore, an accurate error signal can be obtained for the preset control model to control the suspension of the agricultural machine, so that the control accuracy, continuity and reliability of the suspension system are ensured, and the control effect is effectively improved.

Description

Technical Field

[0001] The present invention relates to the field of control algorithms, and in particular to an agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion. Background Art

[0002] With the acceleration of agricultural modernization, the automation and intelligence level of agricultural machinery is constantly improving. As a key component connecting agricultural machinery and tractor head, the performance of agricultural machinery suspension system directly affects the quality and efficiency of agricultural operations. In farmland operations, the ground is often uneven. In order to ensure the consistency of the operating depth of agricultural machinery, it is of great significance to realize the contour control of the suspension system.

[0003] High-precision attitude measurement is required to achieve the contour control of the suspension system. Traditionally, high-precision attitude measurement is achieved using inertial navigation. The angular velocity and linear acceleration during motion are sensed by gyroscopes and accelerometers, and the attitude angle of the moving carrier is obtained through integration and calculation methods. However, the inherent drift and cumulative error problems of inertial navigation lead to poor long-term accuracy, and the low frequency of elevation update leads to poor contour control effect.

[0004] The Global Navigation Satellite System (GNSS) can provide high-precision position and attitude information, while the Inertial Measurement Unit (IMU) can measure the acceleration and angular velocity of an object in real time. The two are highly complementary. On the one hand, the high-precision position and attitude information of GNSS can correct the accumulated errors of IMU, improve long-term accuracy and stability; on the other hand, the high-frequency data of IMU can make up for the low update frequency of GNSS signals, and can still maintain accurate estimation of the attitude of agricultural machinery when the satellite signal is temporarily lost or interfered, ensuring the continuity and reliability of suspension system control, thereby effectively improving the control effect. Summary of the invention

[0005] The purpose of the present invention is to overcome the above-mentioned technical deficiencies and provide an agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion to solve the problem of low elevation update frequency in existing attitude measurement technology and the profiling problem faced by agricultural implements in hilly and mountainous areas, thereby ensuring the continuity and reliability of suspension system control and effectively improving the control effect.

[0006] In order to achieve the above technical objectives, in a first aspect, the technical solution of the present invention provides an agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion, which is applied to an agricultural machinery with a head and agricultural implements, both of which are equipped with a GNSS module and an IMU module, and comprises the following steps:

[0007] Acquire first installation position data and second installation position data, and a first lever arm value corresponding to the first installation position data and a second lever arm value corresponding to the second installation position, and acquire first geographic location data, second geographic location data, first elevation data, second elevation data, first state data, and second state data, wherein the first installation position data and the second installation position data are respectively used to characterize the position coordinates of the GNSS modules of the machine head and the agricultural implement, the first geographic location data and the second geographic location data are respectively used to characterize the positions of the machine head and the agricultural implement in the geodetic coordinate system, the first elevation data and the second elevation data are respectively used to characterize the altitudes of the machine head and the agricultural implement, and the first state data and the second state data are respectively used to characterize the current attitudes and accelerations of the machine head and the agricultural implement acquired through the IMU module;

[0008] Obtaining first high-precision center-of-gravity coordinate data according to the first geographic location data, the first state data, the first installation position data and the first lever arm value, and obtaining second high-precision center-of-gravity coordinate data according to the second geographic location data, the second state data, the second installation position data and the second lever arm value, wherein the first high-precision center-of-gravity coordinate data and the second high-precision center-of-gravity coordinate data are used to represent the two-dimensional position coordinates of the machine head and the agricultural implement, respectively;

[0009] Performing data fusion processing on the first elevation data and the first posture data to obtain first high-precision elevation data, and then obtaining profiling data according to the first high-precision elevation data and the first high-precision center of gravity coordinate data, wherein the profiling data is used to characterize the motion path of the center of gravity of the nose and the corresponding three-dimensional position coordinates;

[0010] Performing data fusion processing on the second elevation data and the second posture data to obtain second high-precision elevation data, and then obtaining real-time status data according to the second high-precision elevation data and the second high-precision center of gravity coordinate data, wherein the real-time status data is used to characterize the three-dimensional position coordinates of the current center of gravity of the agricultural implement;

[0011] A first error signal is obtained according to the profiling data and the real-time status data, and the first error signal is input into a preset control model to obtain a first control signal and control the suspension of the agricultural machinery to extend and retract at different positions according to the first control signal to achieve profiling control.

[0012] Compared with the prior art, the beneficial effects of the present invention include:

[0013] Traditional inertial navigation can only solve the problem of automatic leveling but not the problem of contour tracking. Traditional elevation control is not robust due to its low update frequency. The technical solution of the present invention can make use of the high-frequency data of IMU to compensate for the low update frequency of GNSS signals by fusing the signals of GNSS and IMU. The high-precision elevation data of both are obtained by respectively setting the GNSS module and IMU module on the head of the agricultural machinery and the agricultural implements, and the precise three-dimensional coordinates of the center of gravity of the head and the agricultural implements can be obtained. Therefore, accurate error signals can be obtained for the preset control model to control the suspension of the agricultural machinery to ensure the accuracy, continuity and reliability of the suspension system control, thereby effectively improving the control effect.

[0014] In a second aspect, the technical solution of the present invention provides an agricultural machinery, which is provided with a head and an agricultural implement, and the head and the agricultural implement are both equipped with a GNSS module and an IMU module, and the agricultural machinery is used to execute the agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion as mentioned in the embodiment of the first aspect of the present invention.

[0015] In a third aspect, the technical solution of the present invention provides a computer-readable storage medium, which stores computer-executable instructions, and the computer-executable instructions are used to enable a computer to execute an agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion as any one of the first aspects of the present invention.

[0016] The embodiment of the present invention includes: obtaining first installation position data and second installation position data and a first lever arm value corresponding to the first installation position data and a second lever arm value corresponding to the second installation position, and obtaining first geographic location data, second geographic location data, first elevation data, second elevation data, first state data and second state data, wherein the first installation position data and the second installation position data are respectively used to characterize the position coordinates of the GNSS modules of the machine head and the agricultural implement, the first geographic location data and the second geographic location data are respectively used to characterize the positions of the machine head and the agricultural implement in the geodetic coordinate system, the first elevation data and the second elevation data are respectively used to characterize the altitudes of the machine head and the agricultural implement, and the first state data and the second state data are respectively used to characterize the current posture and acceleration of the machine head and the agricultural implement obtained by the IMU module; obtaining first high-precision center of gravity coordinate data according to the first geographic location data, the first state data, the first installation position data and the first lever arm value, and obtaining first high-precision center of gravity coordinate data according to the second geographic location data, the second state data, the second ... second lever arm value. The first high-precision center of gravity coordinate data is obtained by combining the position data of the machine head and the second arm value, wherein the first high-precision center of gravity coordinate data and the second high-precision center of gravity coordinate data are respectively used to characterize the two-dimensional position coordinates of the machine head and the agricultural implement; the first elevation data and the first posture data are subjected to data fusion processing to obtain the first high-precision elevation data, and then the profiling data is obtained according to the first high-precision elevation data and the first high-precision center of gravity coordinate data, wherein the profiling data is used to characterize the motion path of the center of gravity of the machine head and the corresponding three-dimensional position coordinates; the second elevation data and the second posture data are subjected to data fusion processing to obtain the second high-precision elevation data, and then the real-time status data is obtained according to the second high-precision elevation data and the second high-precision center of gravity coordinate data, wherein the real-time status data is used to characterize the three-dimensional position coordinates of the center of gravity of the current agricultural implement; a first error signal is obtained according to the profiling data and the real-time status data, the first error signal is input into a preset control model, a first control signal is obtained, and the suspension of the agricultural machinery is controlled to be extended and retracted at different positions according to the first control signal to realize profiling control. According to the solution provided by the embodiment of the present invention, by fusing the signals of GNSS and IMU, the high-frequency data of IMU can be used to compensate for the low updating frequency of GNSS signals, and the high-precision elevation data of the two can be obtained by respectively arranging the GNSS module and IMU module on the head of the agricultural machinery and the agricultural implement, so that the accurate three-dimensional coordinates of the center of gravity of the head and the agricultural implement can be obtained. Therefore, the accurate error signal can be obtained for the preset control model to control the suspension of the agricultural machinery to ensure the accuracy, continuity and reliability of the suspension system control, thereby effectively improving the control effect.

[0017] Other features and advantages of the present invention will be described in the following description, and partly become apparent from the description, or understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained by the contents particularly pointed out in the description, claims and drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] The accompanying drawings are used to provide a further understanding of the technical solution of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the technical solution of the present invention and do not constitute a limitation on the technical solution of the present invention.

[0019] Figure 1 It is a schematic diagram of agricultural machinery and implements and GNSS / IMU installation provided by one embodiment of the present invention;

[0020] Figure 2 It is the coordinate system conversion diagram of the present invention;

[0021] Figure 3 It is a schematic diagram of the relationship between the position point of the agricultural machinery vehicle and the body posture in the navigation coordinate system;

[0022] Figure 4 It is a flow chart of an agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion provided by an embodiment of the present invention;

[0023] Figure 5 It is a comparison diagram of the original GNSS elevation value and the GNSS / IMU fusion filtered value processed by the Kalman filter provided by a test example of the present invention;

[0024] Figure 6 It is a schematic diagram of the elevation control effect of an agricultural machinery of a test example of the present invention using the agricultural machinery suspension profiling control algorithm of the present invention;

[0025] Figure 7 It is an algorithm block diagram of an agricultural machine realizing profiling control and automatic leveling through PID control provided by a test example of the present invention; DETAILED DESCRIPTION

[0026] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with 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 intended to limit the present invention.

[0027] It should be noted that, although the functional modules are divided in the system schematic diagram and the logical order is shown in the flowchart, in some cases, the steps shown or described may be performed in a different order than the module division in the system or the order in the flowchart. The terms "first", "second", etc. in the specification, claims and the above drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence.

[0028] The embodiment of the present invention provides an agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion, which is applied to an agricultural machinery provided with a machine head and agricultural implements, wherein the machine head and the agricultural implements are both equipped with a GNSS module and an IMU module, and the agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion comprises the following steps: obtaining first installation position data and second installation position data, as well as a first lever arm value corresponding to the first installation position data and a second lever arm value corresponding to the second installation position, and obtaining first geographic location data, second geographic location data, first elevation data, second elevation data, first state data, and second state data, wherein the first installation position data and the second installation position data are respectively used to characterize the position coordinates of the GNSS modules of the machine head and the agricultural implements, the first geographic location data and the second geographic location data are respectively used to characterize the positions of the machine head and the agricultural implements in the geodetic coordinate system, the first elevation data and the second elevation data are respectively used to characterize the altitudes of the machine head and the agricultural implements, and the first state data and the second state data are respectively used to characterize the current posture and acceleration of the machine head and the agricultural implements acquired through the IMU module; according to the first geographic location data, the first state data, the first installation position data, the second installation position data, the first elevation data, the second elevation data, the first state data, and the second state data, The method comprises the steps of: obtaining first high-precision center of gravity coordinate data according to the first position data and the first lever arm value, and obtaining second high-precision center of gravity coordinate data according to the second geographical location data, the second state data, the second installation position data and the second lever arm value, wherein the first high-precision center of gravity coordinate data and the second high-precision center of gravity coordinate data are used to characterize the two-dimensional position coordinates of the machine head and the agricultural implement respectively; performing data fusion processing on the first elevation data and the first posture data to obtain first high-precision elevation data, and then obtaining profiling data according to the first high-precision elevation data and the first high-precision center of gravity coordinate data, wherein the profiling data is used to characterize the motion path of the center of gravity of the machine head and the corresponding three-dimensional position coordinates; performing data fusion processing on the second elevation data and the second posture data to obtain second high-precision elevation data, and then obtaining real-time state data according to the second high-precision elevation data and the second high-precision center of gravity coordinate data, wherein the real-time state data is used to characterize the three-dimensional position coordinates of the center of gravity of the current agricultural implement; obtaining a first error signal according to the profiling data and the real-time state data, inputting the first error signal into a preset control model, obtaining a first control signal, and controlling the suspension of the agricultural machinery to extend and retract at different positions according to the first control signal to realize profiling control. According to the solution provided by the embodiment of the present invention, by fusing the signals of GNSS and IMU, the high-frequency data of IMU can be used to compensate for the low updating frequency of GNSS signals, and the high-precision elevation data of the two can be obtained by respectively arranging the GNSS module and IMU module on the head of the agricultural machinery and the agricultural implement, so that the accurate three-dimensional coordinates of the center of gravity of the head and the agricultural implement can be obtained. Therefore, the accurate error signal can be obtained for the preset control model to control the suspension of the agricultural machinery to ensure the accuracy, continuity and reliability of the suspension system control, thereby effectively improving the control effect.

[0029] The embodiments of the present invention are further described below in conjunction with the accompanying drawings.

[0030] Figure 1 FIG. 1 is a schematic diagram of an agricultural machine and a GNSS / IMU installation provided by an embodiment of the present invention. Figure 1 In the example of , the agricultural machine includes a head 100 and an agricultural implement 200, the agricultural implement 200 is moved by being pulled by the head, and the head 100 and the agricultural implement 200 are both provided with a sensor part 300, and the sensor part 300 is provided with an IMU module and a GNSS module. The GNSS module provided on the head 100 can provide first position data and first elevation data for characterizing the position and elevation of the head 100, and the IMU module provided on the head 100 can provide first state data for characterizing the current acceleration and posture of the head.

[0031] The GNSS module provided on the agricultural implement 200 can provide second position data and second elevation data for characterizing the position and elevation of the agricultural implement 200 , and the IMU module provided on the agricultural implement 200 can provide second state data for characterizing the current acceleration and posture of the head 100 .

[0032] In one embodiment, the GNSS module and the IMU module are integrated into the same module.

[0033] In one embodiment, the GNSS module may be a RTK-GNSS receiver.

[0034] In one embodiment, the first position data and the second position data are coordinates in a geodetic coordinate system.

[0035] Reference Figure 2 , Figure 2 is the coordinate system conversion diagram of the present invention, Figure 2 The geodetic coordinate system is shown in the figure, also known as the Earth-centered Earth-fixed geodetic coordinate system, referred to as the e-system, which is the reference coordinate system for GNSS to determine the spatial position coordinates on the earth's surface. This coordinate system uses a mathematical ellipsoid to replace the irregularly shaped earth's surface to represent the mathematical reference system for the spatial position on the earth's surface, also known as the ellipsoidal coordinate system. The origin is located at the center of the ellipsoid and consists of three reference reference surfaces: the prime meridian plane, the earth's equatorial plane, and the geodetic datum (or geoid). The spatial point refers to the three reference surfaces to determine the position coordinates (B, L, H), which are longitude, latitude, and elevation respectively. Longitude (B) is the dihedral angle between the meridian plane where the spatial point is located and the prime meridian plane, latitude (L) is the line angle between the line connecting the spatial point and the origin of the coordinate system and the earth's equatorial plane, and elevation is the distance from the spatial point to the nearest point on the geodetic datum (geoid).

[0036] Reference Figure 2, the local tangent plane coordinate system is also referred to as the T system. It is a plane coordinate system obtained by transforming the local ellipsoid in the E system through plane projection, and is usually completed using Gaussian projection. The T system is used in the present invention to determine the reference coordinate system for parameters such as the position coordinates, speed and heading expression of the agricultural machinery head and agricultural implements, and is also called the trajectory reference coordinate system; and because the T system is the reference system for the navigation control and operation of agricultural machinery, it can also be called the navigation coordinate system. Specifically, the definition of the navigation coordinate system in the present invention is as follows: the origin O is located near the range of motion of the agricultural machinery and is fixed to the ground, the X-axis points to the east, the Y-axis points to the north, and the Z-axis is perpendicular to the XY plane. It is a right-handed rectangular coordinate system.

[0037] In one embodiment, the first elevation data and the second elevation data are H parameters in a geodetic coordinate system.

[0038] Reference Figure 3 , is a schematic diagram of the relationship between the position point of the agricultural machinery vehicle and the body posture in the navigation coordinate system, Figure 3 The geographic coordinate system, referred to as the g-system, is shown in the figure. The g-system moves with the carrier in the t-system and serves as the vehicle's attitude reference coordinate system. It is also an important reference system for homogeneous coordinate transformation. The g-system uses the ENU axis and is always parallel to the t-system during the movement with the vehicle. The origin is located at the center of the main positioning antenna of the navigation and positioning system carried by the nose or agricultural implement, and is not fixed.

[0039] Figure 3 Also shown is the carrier coordinate system, referred to as the b system, which is a reference coordinate system used to represent the positional relationship between different points of the agricultural machinery vehicle. In one embodiment of the present invention, the origin of the carrier coordinate system coincides with the origin of the geographic coordinate system to facilitate the calculation of homogeneous coordinate transformations, which are defined as follows: the coordinate origin O is located at the center of the main positioning antenna of the GNSS module and is fixedly connected to it, the X-axis points to the longitudinal direction of the vehicle, the Y-axis points to the lateral direction of the vehicle, and the Z-axis points to the vertical axis direction of the carrier. The clear definition of the b system also determines the installation orientation of the IMU at the center of gravity of the carrier, that is, the heading reference coordinate system is parallel to the carrier coordinate system.

[0040] In one embodiment, the first state data and the second state data respectively represent the current acceleration information and attitude angle information measured by the IMU modules on the machine head and the agricultural implement.

[0041] The agricultural machinery and application scenarios described in the embodiments of the present invention are intended to more clearly illustrate the technical solutions of the embodiments of the present invention, and do not constitute a limitation on the technical solutions provided by the embodiments of the present invention. Those skilled in the art can appreciate that with the evolution of agricultural machinery and the emergence of new application scenarios, the technical solutions provided by the embodiments of the present invention are also applicable to similar technical problems.

[0042] It can be understood by those skilled in the art that Figure 1The agricultural machinery shown in the figure does not constitute a limitation on the embodiments of the present invention, and may include more or fewer components than shown in the figure, or combine certain components, or arrange the components differently.

[0043] Based on the above agricultural machinery, various embodiments of the agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion of the present invention are proposed below.

[0044] Reference Figure 4 , Figure 4 It is a flow chart of an agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion provided by an embodiment of the present invention.

[0045] exist Figure 4 In the example, the agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion of an embodiment of the present invention includes but is not limited to step S100, step S200, step S300, step S400 and step S500.

[0046] Step S100, obtaining first installation position data and second installation position data, a first lever arm value corresponding to the first installation position data and a second lever arm value corresponding to the second installation position data, and obtaining first geographic location data, second geographic location data, first elevation data, second elevation data, first state data and second state data.

[0047] Specifically, the first installation position data and the second installation position data are respectively used to characterize the position coordinates of the GNSS modules of the machine head and the agricultural implement, the first geographical location data and the second geographical location data are respectively used to characterize the positions of the machine head and the agricultural implement in the geodetic coordinate system, the first elevation data and the second elevation data are respectively used to characterize the altitudes of the machine head and the agricultural implement, and the first state data and the second state data are respectively used to characterize the current posture and acceleration of the machine head and the agricultural implement obtained through the IMU module.

[0048] Specifically, in the process of converting the first geographic location data and the second geographic location data provided by GNSS in the geodetic coordinate system to the center of gravity position of the vehicle head and the center of gravity position of the agricultural implement in the carrier coordinate system, the lever arm value provides important compensation information. The lever arm value actually represents the offset between the GNSS installation position and the center of gravity position of the vehicle head in the carrier coordinate system.

[0049] Specifically, the first arm value can be obtained by measuring the distance from the center of the main positioning antenna of the GNSS module set on the front of the vehicle to the center of gravity of the front of the vehicle; the second arm value can be obtained by measuring the distance from the center of the main positioning antenna of the GNSS module set on the agricultural implement to the center of gravity of the agricultural implement.

[0050] Step S200, obtaining first high-precision center-of-gravity coordinate data according to the first geographic location data, the first state data, the first installation position data and the first lever arm value, and obtaining second high-precision center-of-gravity coordinate data according to the second geographic location data, the second state data, the second installation position data and the second lever arm value.

[0051] Specifically, the first high-precision center-of-gravity coordinate data and the second high-precision center-of-gravity coordinate data are respectively used to represent the two-dimensional position coordinates of the machine head and the agricultural implement on the XY axis in the navigation coordinate system.

[0052] Step S300, performing data fusion processing on the first elevation data and the first posture data to obtain first high-precision elevation data, and then obtaining profiling data according to the first high-precision elevation data and the first high-precision center of gravity coordinate data.

[0053] Specifically, the first elevation data represents the coordinates of the center of gravity of the nose on the Z axis in the navigation coordinate system, and the profiling data is used to characterize the motion path and corresponding three-dimensional position coordinates of the center of gravity of the nose in the navigation coordinate system.

[0054] Step S400, performing data fusion processing on the second elevation data and the second posture data to obtain second high-precision elevation data, and then obtaining real-time status data according to the second high-precision elevation data and the second high-precision center of gravity coordinate data.

[0055] Specifically, the second elevation data represents the coordinates of the center of gravity of the agricultural implement on the Z axis in the navigation coordinate system, and the real-time status data is used to represent the corresponding three-dimensional position coordinates of the center of gravity of the head in the navigation coordinate system.

[0056] Step S500, obtaining a first error signal according to the profiling data and the real-time status data, inputting the first error signal into a preset control model, obtaining a first control signal and controlling the suspension of the agricultural machinery to extend and retract at different positions according to the first control signal to achieve profiling control.

[0057] It can be understood that the technical solution of the present invention can make use of the high-frequency data of the IMU to compensate for the low update frequency of the GNSS signal by fusing the signals of the GNSS and IMU, and obtain high-precision elevation data of the two by means of the GNSS module and the IMU module respectively arranged on the head of the agricultural machinery and the agricultural implement, so as to obtain the accurate three-dimensional coordinates of the center of gravity of the head and the agricultural implement. Therefore, it is possible to obtain accurate error signals for the preset control model to control the suspension of the agricultural machinery to ensure the accuracy, continuity and reliability of the suspension system control, thereby effectively improving the control effect.

[0058] Specifically, the first error signal may be the difference in Z-axis coordinates when the XY-axis coordinates represented by the real-time status data and the XY-axis coordinates represented by the profiling data are the same in the navigation coordinate system.

[0059] Furthermore, the first error signal may also be the difference between a difference in Z-axis coordinate and a preset value when the XY-axis coordinates represented by the real-time status data and the XY-axis coordinates represented by the profiling data are the same in the navigation coordinate system.

[0060] In one embodiment, step S100 includes:

[0061] Acquire first installation position data and second installation position data;

[0062] A first lever arm value is obtained according to preset first center of gravity coordinate data and first installation position data, and a second lever arm value is obtained according to preset second center of gravity coordinate data and second installation position data, wherein the first center of gravity position data and the second center of gravity position data respectively represent the three-dimensional position coordinates of the machine head and the agricultural implement.

[0063] It can be understood that the first center of gravity coordinate data and the second center of gravity coordinate data may be the center of gravity coordinates of the machine head and the agricultural implement in each coordinate system, and the embodiment of the present invention does not specifically limit this.

[0064] In one embodiment, step S200 includes:

[0065] Performing Euler projection conversion processing on the first geographical location data, the first state data and the first lever arm value to obtain first high-precision center of gravity coordinate data;

[0066] The second geographical location data and the second state data and the second lever arm value are processed by Euler projection transformation to obtain second high-precision center of gravity coordinate data.

[0067] Specifically, the first geographic location data and the second geographic location data represent coordinates of the centers of the main positioning antennas of the two GNSS modules in the geodetic coordinate system.

[0068] The Euler projection conversion process includes:

[0069] Based on Gauss projection, the coordinates of the first geographic location data and the second geographic location data in the geodetic coordinate system are converted into coordinates in the navigation coordinate system, which is expressed by the formula:

[0070]

[0071] Among them, the geodetic coordinate system, also known as the e system, has position coordinates expressed as (B, L, H), where B is longitude, L is latitude, and H is elevation;

[0072] The navigation coordinate system, also known as the T system, is a plane coordinate system obtained by transforming the local ellipsoid in the geodetic coordinate system through plane projection. Its origin is located near the range of motion of the agricultural machinery and is fixed to the ground. The X axis points to the east, the Y axis points to the north, and the Z axis is perpendicular to the XY plane. It is a right-hand rectangular coordinate system.

[0073] Geographic coordinate system, also known as G system, moves with the agricultural machinery in the navigation coordinate system. The geographic coordinate system uses the ENU axis, and its origin is located at the center of the main positioning antenna of the GNSS module and is not fixed to it.

[0074] The carrier coordinate system, also known as the b system, is a reference coordinate system used to describe the positional relationship between different points of the agricultural machinery. Its origin is located at the center of the main positioning antenna of the GNSS module and is fixed to it. The X-axis points to the longitudinal direction of the agricultural machinery, the Y-axis points to the transverse direction of the agricultural machinery, and the Z-axis points to the vertical axis direction of the agricultural machinery.

[0075] (x G ,y G ) NED It is the coordinate of the main positioning antenna of the GNSS module in the geographic coordinate system;

[0076] (x G0 ,y G0 ) NED It is the coordinate of the origin of the navigation coordinate system in the geographic coordinate system;

[0077] (x t ,y t ,z t ) ENU It is the coordinate of the main positioning antenna position of the GNSS module in the navigation coordinate system.

[0078] Specifically, the Euler projection conversion process also includes:

[0079] Define the position vectors of the center of gravity of the head and the center of gravity of the implement in the carrier coordinate system according to the first lever arm value and the second lever arm value;

[0080] Based on the homogeneous coordinate transformation, the coordinates of the center of gravity of the head and the center of gravity of the implement in the navigation coordinate system are obtained according to the position vector, the coordinates in the navigation coordinate system represented by the first geographic location data and the second geographic location data, and the attitude angle in the navigation coordinate system represented by the first state data and the second state data, which are expressed by the formula:

[0081]

[0082] in, b p a is the position vector of the center of gravity of the machine head or the center of gravity of the agricultural implement in the carrier coordinate system;

[0083] p aIndicates the position of the center of gravity of the machine head or the center of gravity of the agricultural implement in the carrier coordinate system;

[0084] t p a Yes a The coordinates in the navigation coordinate system, that is, the coordinates of the center of gravity of the machine head or the center of gravity of the agricultural implement in the navigation coordinate system;

[0085] t p bO is a displacement vector, that is, the coordinates represented by the first geographic location data and the second geographic location data in the navigation coordinate system;

[0086] The rotation transformation matrix is ​​constructed based on the Euler angle orientation description of the b system relative to the g system. Its physical meaning is that when the g system is rotated along the three coordinate axes in sequence to coincide with the b system, the coordinate value of the vector whose three-axis coordinates in the g system are all 1 in the b coordinate system.

[0087] In one embodiment of the present invention, the orientation of the agricultural machine in the b-frame is described based on the attitude angles (φ, θ, ψ, i.e., the rolling angle φ of the agricultural machine around the x-axis of the b-frame, the pitch angle θ of the y-axis, and the heading angle ψ of the z-axis; and the attitude angle itself describes the orientation of the coordinate system formed by the rotation angle of the b-frame in the g-frame state along the XYZ specific sequence process.

[0088] In one embodiment of the present invention, the IMU module is Mti-300 produced by Xsens, and the roll angle, pitch angle and heading angle measured and outputted by the IMU module are the attitude angles of the vehicle body. That is, it is obtained through the process described in the above formula, that is, the rotation matrix of the corresponding axis is multiplied on the left in the rotation order.

[0089] because The rotation transformation matrix is ​​constructed based on the Euler angle description of the carrier coordinate system relative to the geographic coordinate system. The acquisition process is expressed by the formula:

[0090]

[0091]

[0092] Among them, the attitude angle (φ, θ, ψ) is the roll angle of the x-axis, the pitch angle of the y-axis, and the heading angle of the z-axis around the carrier coordinate system, and the attitude angle is obtained from the first state data and the second state data.

[0093] In one embodiment, the heading angle ψ used when adopting the homogeneous coordinate transformation is based on the single-antenna heading information measured by UM980.

[0094] In addition, because the measurement data of the MTi-300 unit has certain errors in a dynamic environment, an embodiment of the present invention processes the acquired data through a weighted average filter, and the filter form is as follows:

[0095]

[0096] In the formula, C k is the weighting coefficient, K is the number of samples, and X(k) is the sample value involved in filtering.

[0097] In one embodiment, the update frequency of the UM980 module is 20HZ, and the update frequency of the MTi-300 unit is 100HZ. In order to ensure that the acquisition of position, attitude, and inertial data is as consistent as possible in time, the number of samples N for weighted average filtering of attitude and inertial data is set to 4, and the weighting coefficients C1 to C4 are 0.1, 0.2, 0.3, and 0.4 respectively.

[0098] In one embodiment, step S300 includes:

[0099] Performing data fusion processing on the first elevation data and the first posture data based on the Kalman filter to obtain first high-precision elevation data;

[0100] Combining the first high-precision barycenter coordinate data and the first high-precision elevation data to obtain profiling data;

[0101] The state equation and observation equation of the Kalman filter are expressed as:

[0102] Equation of state:

[0103]

[0104] Observation equation:

[0105] Z k =[1 0]X k +w a

[0106] Among them, is the state variable at time k, is the state variable at time k-1, is the update frequency, is the Z-axis acceleration value measured by the MTi-300 module at time k, is the observation value at time k, is the error term output by the MTi-300 module at time k, is the Gaussian white noise measured by the MTi-300 module, is the mean Gaussian white noise with zero derivative of the error term output by the MTi-300 module, and is the measured Gaussian white noise for calculating the elevation H using Newton's second law.

[0107] Specifically, during the driving process of agricultural machinery, changes in terrain will cause changes in the elevation of the agricultural machinery. The acceleration measured by the MTi-300 on the agricultural machinery is the rate of change of the elevation of the agricultural machinery C:

[0108] Integrating over time gives the change in elevation:

[0109] ΔH=∫(C)dt

[0110] However, since the output of the MTi-300 accelerometer contains error terms such as random drift and zero bias, recorded as ξ, the integral calculation of the elevation change will introduce the error term of the accelerometer, and the accumulation over time will cause the elevation change measurement value to diverge, which cannot be used for elevation feedback of leveling and contour control. The true height value can be expressed as:

[0111] ΔH t =∫(C-ξ)dt

[0112] Taking the derivatives of both sides, we have:

[0113]

[0114] w g is the Gaussian white noise measured by the accelerometer. In general, the error term ξ of the MTi-300 accelerometer is a slow variable that changes with time, and its derivative is a zero-mean Gaussian white noise, denoted as

[0115] The measured value of the elevation change is calculated by Newton's second law, and its measured value H is calculated by the true elevation value H t and measure Gaussian white noise w a Composition:

[0116] ΔH=ΔH t +w g

[0117] In one embodiment, the iterative calculation process of establishing a Kalman filter to solve the elevation includes:

[0118] 1. Time update estimate:

[0119] (1) State variable estimation at time k:

[0120]

[0121] In the formula, is the prior state estimate at time k, is the posterior state estimate at time k-1, This formula uses the estimated state variable at time k-1 The state variables at time k are estimated a priori using the input control quantity of the system.

[0122] (2) Covariance estimation at time k:

[0123] P k - =AP k-1 AT +Q

[0124] Where P k - is the covariance estimate of the prior estimation error at time k, P k-1 is the covariance estimate of the posterior estimation error at time k-1, Q is the process excitation noise covariance matrix, q1 is the covariance of the accelerometer measurement error, and q2 is the covariance of the estimated accelerometer bias. This formula estimates the covariance of the posterior estimation error at time k-1, P k-1 The k-time covariance is estimated by the process excitation noise covariance matrix Q.

[0125] The two formulas in the time update estimation part are responsible for calculating the estimated values ​​of the current state variables and error covariance forward in time in order to construct a priori estimates for the next moment.

[0126] 2. Measurement update calibration part:

[0127] (1) Kalman gain calculation at time k:

[0128]

[0129] In the formula, K k is the Kalman gain, the observation matrix H = [1 0], R is the observation noise covariance matrix, R = [r1], r1 is the covariance of the vehicle height change measurement error. This formula estimates the covariance of the prior estimation error at time k P k - , the observation matrix H and the observation noise covariance matrix R are used to calculate the Kalman gain at time k, with the aim of minimizing the posterior estimation error covariance to change the weights of the state variable estimates and measurements.

[0130] (2) Correction update of state variables at time k:

[0131]

[0132] In the formula, is the posterior state estimate calculated at time k, is the prior state estimate at time k, Z k is the observed value of the elevation at time k. This formula is obtained by Kalman gain K k The difference between the observed value at time k and the prior state estimate is used to correct and update the prior state estimate. That is, the linear combination of the weighted observed value and the estimated value constitutes the posterior state estimate at the current moment. According to this formula, the elevation change value after Kalman filtering can be obtained.

[0133] (3) Update the covariance estimate of the posterior estimation error at time k:

[0134]

[0135] In the formula, I is the unit matrix. The covariance estimate of the posterior estimation error is updated through this formula to obtain the optimal estimate of the covariance, so that the Kalman filter continues to run until the system stops.

[0136] Reference Figure 5 , Figure 5 It is a comparison diagram of the GNSS original elevation value and the GNSS / IMU fusion filter value processed by the Kalman filter provided by a test example of the present invention. The test example applies the agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion of the present invention to the agricultural machinery provided by the present invention; Figure 4 The comparison results of the original GNSS elevation value measured by the same GNSS module in the test example and the GNSS / IMU fusion filter value processed by the Kalman filter are shown. The results show that after processing by the Kalman filter, it is obvious that the elevation data with less noise, more stable and continuous can be obtained.

[0137] In one embodiment, the preset control model is a PID controller, and step S500 includes:

[0138] The method comprises: obtaining a first error signal according to the profiling data and the real-time status data, inputting the first error signal into a PID controller, obtaining a first control signal, and controlling the suspension of the agricultural machine to extend and retract at different positions according to the first control signal to realize profiling control, including:

[0139] The difference between the coordinates represented by the profiling data and the real-time status data on the Z axis when they are located at the same position on the XY plane in the navigation coordinate system is used as an error signal;

[0140] The first error signal is input into a preset profiling control model to obtain a first control signal and the suspension of the agricultural machinery is controlled to extend and retract at different positions according to the first control signal to achieve profiling control.

[0141] It can be understood that the center of gravity of the agricultural machinery head and the agricultural implements are selected as reference calculation points, and the Kalman filter is used to fuse the elevation data obtained by the GNSS receiver and the Z-axis acceleration speed obtained by the IMU to obtain accurate estimates of the elevation data of the agricultural machinery head and the agricultural implements. The Euler projection is used to fuse the position data obtained by the GNSS receiver and the attitude data obtained by the IMU to calculate the precise coordinates of the center of gravity of the agricultural machinery head and the agricultural implements. According to the results of the above two calculations, the head data is used to generate high-precision terrain information and the elevation change of the head as a reference benchmark, and the agricultural implement data is used as the input value of the leveling and profiling control as a reference for closed-loop feedback. The PID algorithm is used to perform profiling control of the suspension cylinder for agricultural implements to meet the high flatness requirements of ridge formation, film laying, transplanting and other links in hilly and mountainous areas.

[0142] It is understandable that the control of the suspension cylinder can also be applied to automatic leveling of agricultural machinery.

[0143] Reference Figure 6 , Figure 6 This is a schematic diagram of the elevation control effect of an agricultural machinery of a test example of the present invention using the agricultural machinery suspension profiling control algorithm of the present invention, Figure 6 It is shown in the figure that in the process of profiling elevation control, after applying the agricultural machinery suspension profiling control algorithm of the present invention, the expected elevation change and the actual elevation change have excellent fitting characteristics.

[0144] The agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion in the embodiment of the present invention may also include:

[0145] Obtaining a second error signal according to the first state data and the second state data;

[0146] The second error signal is input into the PID controller to obtain a second control signal and the pitch angle and roll angle between the machine head and the agricultural implement are controlled according to the second control signal to achieve automatic leveling.

[0147] Specifically, the second error signal is the difference between the attitude angle of the nose and the attitude angle of the agricultural implement.

[0148] It is understandable that the second error signal may also be the difference between the attitude angle of the nose and the attitude angle of the implement and a preset value, that is, the purpose of PID control based on the second error signal is to prevent the pitch angle and roll angle between the nose and the implement from exceeding the preset value.

[0149] It is understandable that the PID controller can also receive the first error signal and the second error signal simultaneously to simultaneously control the extension and retraction of the suspension of the agricultural machine at different positions and control the pitch angle and roll angle between the machine head and the agricultural implement.

[0150] Reference Figure 7 , Figure 7 An experimental example of the present invention provides an algorithm block diagram for realizing contour control and automatic leveling of agricultural machinery through PID control. The agricultural machinery in the experimental example can control the suspension through the leveling cylinder and the leveling support frame. It applies the agricultural machinery suspension contour control algorithm based on GNSS / IMU fusion of the embodiment of the present invention. It realizes the function of GNSS / IMU in the above embodiment through the GNSS / INS position and attitude sensing system. Its PID controller can control the leveling cylinder by controlling the electro-hydraulic proportional valve, and the lateral position sensor and elevation position sensor set in the cylinder detect the current lateral position and elevation position of the cylinder to realize the closed-loop control of the leveling cylinder based on it. It can also control its leveling support frame to level according to the lateral inclination and elevation error measured by the GNSS / INS position and attitude sensing system.

[0151] Therefore, it can be understood that the agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion in the embodiment of the present invention can also be applied to the GNSS / INS system, which is also applicable to the profiling control and automatic leveling of the data measured by the GNSS / INS combined positioning and attitude measurement algorithm.

[0152] In addition, an embodiment of the present invention further provides a computer-readable storage medium, which stores computer-executable instructions. The computer-executable instructions are executed by a processor or a controller, for example, by a processor in the above-mentioned device embodiment or the equipment embodiment, so that the above-mentioned processor can execute the agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion in the above-mentioned embodiment, for example, to execute the above-mentioned Figure 4 The method steps in .

[0153] It will be appreciated by those skilled in the art that all or some of the steps and systems in the disclosed method above may be implemented as software, firmware, hardware and appropriate combinations thereof. Some physical components or all physical components may be implemented as software executed by a processor, such as a central processing unit, a digital signal processor or a microprocessor, or may be implemented as hardware, or may be implemented as an integrated circuit, such as an application specific integrated circuit. Such software may be distributed on a computer-readable medium, which may include a computer storage medium (or a non-transitory medium) and a communication medium (or a temporary medium). As known to those skilled in the art, the term computer storage medium includes volatile and non-volatile, removable and non-removable media implemented in any method or technology for storing information (such as computer-readable instructions, data structures, program modules or other data). Computer storage media include, but are not limited to, RAM, ROM, EEPROM, flash memory or other memory technologies, CD-ROM, digital versatile disks (DVD) or other optical disk storage, magnetic cassettes, magnetic tapes, disk storage or other magnetic storage devices, or any other medium that may be used to store desired information and may be accessed by a computer. Furthermore, it is well known to those skilled in the art that communication media typically embodies computer readable instructions, data structures, program modules, or other data in a modulated data signal such as a carrier wave or other transport mechanism, and may include any information delivery media.

[0154] The above is a specific description of the preferred implementation of the present invention, but the present invention is not limited to the above-mentioned implementation mode. Technical personnel familiar with the field can also make various equivalent deformations or substitutions without violating the spirit of the present invention. These equivalent deformations or substitutions are all included in the scope defined by the claims of the present invention.

Claims

1. The agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion is applied to an agricultural machinery with a machine head and agricultural implements, wherein both the machine head and the agricultural implements are equipped with a GNSS module and an IMU module, and is characterized in that: The agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion includes the following steps: Acquire first installation position data and second installation position data, and a first lever arm value corresponding to the first installation position data and a second lever arm value corresponding to the second installation position, and acquire first geographic location data, second geographic location data, first elevation data, second elevation data, first state data, and second state data, wherein the first installation position data and the second installation position data are respectively used to characterize the position coordinates of the GNSS modules of the machine head and the agricultural implement, the first geographic location data and the second geographic location data are respectively used to characterize the positions of the machine head and the agricultural implement in the geodetic coordinate system, the first elevation data and the second elevation data are respectively used to characterize the altitudes of the machine head and the agricultural implement, and the first state data and the second state data are respectively used to characterize the current attitudes and accelerations of the machine head and the agricultural implement acquired by the IMU module; Obtaining first high-precision center-of-gravity coordinate data according to the first geographic location data, the first state data, the first installation position data and the first lever arm value, and obtaining second high-precision center-of-gravity coordinate data according to the second geographic location data, the second state data, the second installation position data and the second lever arm value, wherein the first high-precision center-of-gravity coordinate data and the second high-precision center-of-gravity coordinate data are used to represent the two-dimensional position coordinates of the machine head and the agricultural implement, respectively; Performing data fusion processing on the first elevation data and the first posture data to obtain first high-precision elevation data, and then obtaining profiling data according to the first high-precision elevation data and the first high-precision center of gravity coordinate data, wherein the profiling data is used to characterize the motion path of the center of gravity of the nose and the corresponding three-dimensional position coordinates; Performing data fusion processing on the second elevation data and the second posture data to obtain second high-precision elevation data, and then obtaining real-time status data according to the second high-precision elevation data and the second high-precision center of gravity coordinate data, wherein the real-time status data is used to represent the three-dimensional position coordinates of the current center of gravity of the agricultural implement; A first error signal is obtained according to the profiling data and the real-time status data, the first error signal is input into a preset control model, a first control signal is obtained, and the suspension of the agricultural machinery is controlled to be extended or retracted at different positions according to the first control signal to achieve profiling control.

2. The agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion according to claim 1 is characterized in that: The acquiring of the first installation position data and the second installation position data and a first lever arm value corresponding to the first installation position data and a second lever arm value corresponding to the second installation position data comprises: Acquire the first installation position data and the second installation position data; The first lever arm value is obtained according to the preset first center of gravity coordinate data and the first installation position data, and the second lever arm value is obtained according to the preset second center of gravity coordinate data and the second installation position data, wherein the first center of gravity position data and the second center of gravity position data respectively represent the three-dimensional position coordinates of the machine head and the agricultural implement.

3. The agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion according to claim 1 is characterized in that: The step of obtaining first high-precision center-of-gravity coordinate data according to the first geographic location data, the first state data, the first installation position data, and the first lever arm value, and obtaining second high-precision center-of-gravity coordinate data according to the second geographic location data, the second state data, the second installation position data, and the second lever arm value comprises: Performing Euler projection conversion processing on the first geographical location data, the first state data and the first lever arm value to obtain first high-precision center of gravity coordinate data; The second geographical location data, the second state data and the second lever arm value are processed by Euler projection transformation to obtain second high-precision center of gravity coordinate data.

4. The agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion according to claim 3 is characterized in that: The first geographic location data and the second geographic location data represent the coordinates of the centers of the main positioning antennas of the two GNSS modules in the geodetic coordinate system, and the Euler projection conversion process includes: Based on Gauss projection, the coordinates of the first geographic location data and the second geographic location data in the geodetic coordinate system are converted into coordinates in the navigation coordinate system, which is expressed by the formula: Among them, the position coordinates of the geodetic coordinate system are expressed as (B, L, H), B is longitude, L is latitude and H is elevation; The navigation coordinate system is a plane coordinate system obtained by transforming the local ellipsoid surface in the geodetic coordinate system through plane projection, the origin of which is located near the range of motion of the agricultural machinery and fixed to the ground, the X-axis points to the east, the Y-axis points to the north, and the Z-axis is perpendicular to the XY plane, which is a right-hand rectangular coordinate system; The geographic coordinate system moves with the agricultural machine in the navigation coordinate system, and the geographic coordinate system adopts the ENU axis, and its origin is located at the center of the main positioning antenna of the GNSS module and is not fixed thereto; The carrier coordinate system is a reference coordinate system used to describe the positional relationship between different points of the agricultural machinery. Its origin is located at the center of the main positioning antenna of the GNSS module and is fixed to it. The X-axis points to the longitudinal direction of the agricultural machinery, the Y-axis points to the transverse direction of the agricultural machinery, and the Z-axis points to the vertical axis direction of the agricultural machinery. (x G ,y G)NED is the coordinate of the main positioning antenna position of the GNSS module in the geographic coordinate system; (x G0 ,y G0)NED It is the coordinate of the origin of the navigation coordinate system in the geographic coordinate system; (x t ,y t ,z t)ENU It is the coordinate of the position point of the main positioning antenna of the GNSS module in the navigation coordinate system.

5. The agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion according to claim 4 is characterized in that: The Euler projection conversion process also includes: Define the position vectors of the center of gravity of the head and the center of gravity of the agricultural implement in the carrier coordinate system according to the first lever arm value and the second lever arm value; Based on homogeneous coordinate transformation, the coordinates of the center of gravity of the head and the center of gravity of the implement in the navigation coordinate system are obtained according to the position vector, the coordinates in the navigation coordinate system represented by the first geographical location data and the second geographical location data, and the attitude angles in the navigation coordinate system represented by the first state data and the second state data, which are expressed by the formula: in, b p a is the position vector of the center of gravity of the machine head or the center of gravity of the agricultural implement in the carrier coordinate system; p a Indicates the position of the center of gravity of the machine head or the center of gravity of the agricultural implement in the carrier coordinate system; t p a Yes a The coordinates in the navigation coordinate system, that is, the coordinates of the center of gravity of the machine head or the center of gravity of the agricultural implement in the navigation coordinate system; t p bO is a displacement vector, that is, the coordinates represented by the first geographic location data and the second geographic location data in the navigation coordinate system; The rotation transformation matrix is ​​constructed based on the Euler angle description of the carrier coordinate system relative to the geographic coordinate system. The acquisition process is expressed by the formula: Among them, the attitude angle (φ, θ, ψ) is the roll angle of the x-axis, the pitch angle of the y-axis, and the heading angle of the z-axis around the carrier coordinate system, and the attitude angle is obtained from the first state data and the second state data.

6. The agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion according to claim 1 is characterized in that: The step of performing data fusion processing on the first elevation data and the first posture data to obtain first high-precision elevation data, and then obtaining profiling data according to the first high-precision elevation data and the first high-precision center of gravity coordinate data, comprises: Performing data fusion processing on the first elevation data and the first posture data based on a Kalman filter to obtain first high-precision elevation data; Combining the first high-precision barycenter coordinate data and the first high-precision elevation data to obtain the profiling data; The state equation and observation equation of the Kalman filter are expressed as: Equation of state: Observation equation: Z k =[1 0]X k +w a Among them, X k is the state variable at time k, X k-1 is the state variable at time k-1, Δt is the update frequency, C is the Z-axis acceleration value measured by the IMU module at time k, Z k is the observed value at time k, ξ k is the error term output by the IMU module at time k, w g is the Gaussian white noise measured by the IMU module, w v is the mean Gaussian white noise with zero derivative of the error term output by the IMU module, w a is the measured Gaussian white noise used to calculate the elevation H using Newton's second law.

7. The agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion according to claim 1 is characterized in that: The preset control model is a PID controller; The method of obtaining a first error signal according to the profiling data and the real-time status data, inputting the first error signal into the PID controller, obtaining a first control signal, and controlling the suspension of the agricultural machine to extend and retract at different positions according to the first control signal to achieve profiling control, comprises: taking the difference on the Z axis when the coordinates represented by the profiling data and the real-time status data are located at the same position on the XY plane in the navigation coordinate system as an error signal; The first error signal is input into a preset profiling control model to obtain a first control signal, and the suspension of the agricultural machine is controlled to extend and retract at different positions according to the first control signal to achieve profiling control.

8. The agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion according to claim 7 is characterized in that: Also includes: Obtain a second error signal according to the first state data and the second state data; The second error signal is input into the PID controller to obtain a second control signal, and the pitch angle and roll angle between the machine head and the agricultural implement are controlled according to the second control signal to achieve automatic leveling.

9. An agricultural machine, characterized in that A machine head and an agricultural implement are provided, wherein the machine head and the agricultural implement are both equipped with a GNSS module and an IMU module, and the agricultural machinery is used to execute the agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion as described in any one of claims 1 to 8.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer-executable instructions, and the computer-executable instructions are used to enable a computer to execute the agricultural machinery suspension profiling control algorithm based on GNSS / IMU fusion as described in any one of claims 1 to 8.

Images