A method and device for online calibration of MEMS gyroscope parameters on the front wheel of agricultural machinery

Abstract

The present invention provides a method and device for online calibration of MEMS gyroscope parameters on the front wheels of agricultural machinery. This method relates to the field of inertial measurement units and gyroscope parameter calibration. The method comprises: establishing an error model for the front wheel gyroscope, incorporating the gyroscope's scale factor error and bias into the error model, assuming the bias to be a first-order Markov process, and establishing a corresponding error differential equation; using a normally functioning mainframe inertial navigation system as a reference, utilizing its navigation parameters and the front wheel gyroscope to establish a joint state equation, while simultaneously using the position and velocity information of a satellite navigation system as state constraint observations to construct a joint state Kalman filter; improving system observability through the movement of the agricultural machinery during operation, online calibrating the gyroscope's error parameters using a Kalman filter, and feeding the calibrated parameters back into the gyroscope's output calculation process to achieve real-time online correction of the gyroscope parameters. This method can improve measurement accuracy and long-term stability.

Description

Technical Field

[0001] The present invention relates to the field of inertial measurement unit and gyroscope parameter calibration, and in particular to an online calibration method and device for a MEMS gyroscope parameter of a front wheel of an agricultural machinery. Background Art

[0002] In the development of precision agriculture, automated navigation and driving systems for agricultural machinery are one of the key technologies for achieving this goal. However, with the continuous optimization of land planning and the improvement of modern agricultural machinery operating conditions, the requirements for agricultural machinery operating speed, operating area, and operating accuracy are becoming increasingly stringent. Traditional manual driving methods can no longer meet these requirements. The driver's workload is heavy, and repetitive single driving operations increase driver stress and reduce work efficiency. In addition, the front wheel angle measurement systems of early autonomous driving systems mostly used absolute angle sensors such as Hall effect angle sensors and encoders. These sensors require mechanical modifications to the front axle of the agricultural machinery during installation and a complex calibration process before use. This is not only labor-intensive and time-consuming, but also significantly shortens the sensor's service life in complex and harsh operating environments.

[0003] At present, relevant personnel have studied the possibility of applying gyroscopes as angle measurement sensors in autonomous driving systems. The commonly used devices are MEMS (micro-electromechanical systems) gyroscopes. However, this type of sensor has parameters such as scale factor, zero bias and installation error that need to be compensated through calibration, and after long-term use, there will be parameter changes, resulting in a decrease in measurement accuracy. The common calibration method is to return the device to the factory for recalibration on a two-axis turntable or a three-axis turntable, but this method is not only time-consuming and labor-intensive, but also requires the disassembly and assembly of the equipment, wasting a lot of time and cost. In existing gyroscope angle measurement technology, the angle is usually treated as a separate system state to establish a system equation for filtering and correction. This method has poor single-variable measurement robustness and is easily affected by the environment during field operations, resulting in abnormal or divergent measurement data. Summary of the Invention

[0004] To solve the above-mentioned technical problems, the present invention provides a method and device for online calibration of the parameters of the MEMS gyroscope on the front wheel of agricultural machinery. The method uses the inertial navigation system on the normally operating main body as a reference, utilizes the navigation parameters output by the inertial navigation system, and establishes a joint state equation with the gyroscope to be calibrated installed on the front wheel. Using the position and velocity information of the satellite navigation system as state constraint observations, a joint state Kalman filter is established to collaboratively calibrate the full error parameters of the gyroscope to be calibrated, thereby simplifying the complexity and resource utilization of the vehicle-mounted positioning and front wheel angle calculation system. The present invention combines the gyroscope angle measurement equation with the combined navigation equation, and through the speed, position and expected angle measurement input, increases the system robustness and improves the angle measurement accuracy and reliability.

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

[0006] An online calibration method for parameters of a MEMS gyroscope on a front wheel of an agricultural machine comprises the following steps:

[0007] Step 1: Establish an error model for the front wheel gyroscope, incorporate the scale factor error and zero bias of the front wheel gyroscope into the error model, where the zero bias is assumed to be a first-order Markov process, and establish the corresponding error differential equation;

[0008] Step 2: Using the normally functioning mainframe inertial navigation system as a reference, the navigation parameters of the system and the front wheel gyroscope are used to establish a joint state equation. The position and velocity information of the satellite navigation system are used as state constraint observations to construct a joint state Kalman filter.

[0009] Step 3: Improve the observability of the system through the movement of agricultural machinery during operation. Use the Kalman filter to calibrate the error parameters of the front wheel gyroscope online, and feed the calibrated parameters back to the front wheel gyroscope output calculation process to achieve real-time online correction of the front wheel gyroscope parameters.

[0010] The present invention also provides an online calibration device for the parameters of a MEMS gyroscope on the front wheel of an agricultural machine, comprising the following modules:

[0011] The error differential equation establishment module establishes the error model of the front wheel gyroscope, incorporates the scale factor error and zero bias of the front wheel gyroscope into the error model, where the zero bias is assumed to be a first-order Markov process, and establishes the corresponding error differential equation;

[0012] The filter construction module uses the normally functioning mainframe inertial navigation system as a reference, utilizes its navigation parameters and the front wheel gyroscope to establish a joint state equation, and uses the position and velocity information of the satellite navigation system as state constraint observations to construct a joint state Kalman filter.

[0013] The correction module improves the observability of the system through the movement of agricultural machinery during operation, uses the Kalman filter to calibrate the error parameters of the front wheel gyroscope online, and feeds the calibrated parameters back to the front wheel gyroscope output calculation process to achieve real-time online correction of the front wheel gyroscope parameters.

[0014] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the steps of the above-mentioned method for online calibration of the MEMS gyroscope parameters of the front wheel of agricultural machinery are implemented.

[0015] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the steps of the above-mentioned method for online calibration of the parameters of the MEMS gyroscope of the front wheel of agricultural machinery are implemented.

[0016] Beneficial effects:

[0017] The present invention establishes a joint state equation and observation equation for the front wheel gyroscope parameter model and the main body combined navigation system. By using the speed, position and expected rotation angle measurement inputs and establishing the joint system state equation during agricultural machinery operation, the front wheel angular gyroscope scale factor error and zero bias are calibrated online, increasing the system observability and robustness, thereby improving the long-term stability and reliability of gyro angle measurement. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 The present invention is a flow chart of an online calibration method for the parameters of a MEMS gyroscope on the front wheel of an agricultural machinery.

[0019] Figure 2 This is a graph of angle measurement after gyro error calibration under the simulation conditions of the present invention.

[0020] Figure 3 The figure is a schematic diagram of an online calibration device for a MEMS gyroscope parameter of a front wheel of an agricultural machinery according to the present invention. DETAILED DESCRIPTION

[0021] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only intended to illustrate the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.

[0022] like Figure 1 As shown, the present invention provides an online calibration method for the parameters of a MEMS gyroscope of a front wheel of an agricultural machinery, comprising the following steps:

[0023] Step 1: Establish an error model for the front wheel gyroscope, incorporate the gyroscope's scale factor error and zero bias into the error model, where the zero bias is assumed to be a first-order Markov process, and establish the corresponding error differential equation;

[0024] Step 2: Using the normally functioning mainframe inertial navigation system as a reference, the navigation parameters of the system and the front wheel gyroscope are used to establish a joint state equation. The position and velocity information of the satellite navigation system are used as state constraint observations to construct a joint state Kalman filter.

[0025] Step 3: Improve the observability of the system through the movement of agricultural machinery during operation. Use the Kalman filter to calibrate the gyroscope error parameters online, and feed the calibrated parameters back to the gyroscope output calculation process to achieve real-time online correction of the gyroscope parameters, thereby improving measurement accuracy and long-term stability.

[0026] Specifically, the step 1 includes:

[0027] An error model for the front wheel gyroscope is established. Since a single-axis gyroscope is generally used for front wheel measurement and the installation error measured during installation has good long-term stability, the error model is simplified to reduce the dimension of the subsequent Kalman filter.

[0028] The error model expression of the front wheel gyroscope is:

[0029] ;

[0030] in, Calculate the output error for the gyro, is the gyro scale factor error, is the gyro measurement output, is the gyro zero bias.

[0031] The bias of the MEMS gyroscope is assumed to be a first-order Markov process, so the error differential equation of the bias is:

[0032] ;

[0033] Where, is the inverse time constant coefficient, To excite white noise, Indicates gyro bias The differential of .

[0034] Specifically, the step 2 includes:

[0035] Treating the installation error as a constant, the joint state equation of the main inertial navigation system, front wheel angle measurement, and gyro calibration coefficient is established based on the above error differential equation:

[0036] ;

[0037] ;

[0038] in, is the state variable, is the state variable dimension, is a real number; is the state transfer matrix; is the process noise matrix; is the measured variable, is the observation dimension; is the measurement matrix; and are process noise and measurement noise variables, respectively, and are assumed to be zero-mean Gaussian white noise. is the process noise dimension, Represents the first-order derivative of the state variable x.

[0039] State variables Select as follows:

[0040] ;

[0041] Among them, the intermediate variable ;

[0042] Intermediate variables ;

[0043] Intermediate variables ;

[0044] Intermediate variables ;

[0045] in, 、 and is the attitude angle error, 、 and are the velocity errors in the east, north and sky directions, respectively, 、 and They are latitude error, longitude error and altitude error respectively; 、 and They are the zero bias of the main body's three-axis gyroscope; 、 and They are the zero bias of the three-axis accelerometer of the main body; 、 and Represent the arm errors in the coordinate axis directions respectively; is the wheel steering angle measurement error, is the front wheel gyro calibration coefficient error, is the wheel-mounted gyro bias; the superscript T indicates the transpose of the matrix.

[0046] Process noise , 、 、 、 is the gyroscope random error parameter, 、 、 is the random error parameter of the accelerometer, which is selected according to the noise level of the inertial device.

[0047] State transition matrix and the process noise matrix The specific form is:

[0048] , ;

[0049] in, ; Represents a matrix of p×q with elements of 0, p=1,2,3,6,9, q=1,3,6, is the attitude transfer matrix.

[0050] State transition matrix The sub-matrices in are expressed as follows:

[0051] , ;

[0052] ;

[0053] ;

[0054] ;

[0055] ;

[0056] ;

[0057] ;

[0058] ;

[0059] ;

[0060] Among them, footnote 、 and Respectively represent the east, north and sky direction; is the local latitude; is the height; 、 、 To calculate the speed Three components in the navigation coordinate system; 、 、 Projection of the accelerometer output into the navigation coordinate system. and are the principal curvature radii along the meridian and the zodiac circle, is the Earth's rotation angular rate;

[0061] Measured variables of the system It is the difference between the speed and position of INS (Inertial Navigation System) and GNSS (Global Navigation Satellite System) and the heading angle error of dual GNSS. The specific form is as follows:

[0062] ;

[0063] in, are the positions solved by GNSS and INS respectively, is the lever arm vector, is the attitude angle coordinate transposed matrix. are the speeds of GNSS and INS strapdown solutions, is the carrier rotation angular velocity vector, They are the output headings calculated by dual-antenna GNSS and INS strapdown solutions, are the calculated value and reference value of the front wheel angle respectively.

[0064] Observation matrix ;

[0065] Among them, the middle vector ;

[0066] Intermediate vector ;

[0067] Intermediate vector ;

[0068] Among them, diag() represents a diagonal matrix, is the attitude rate, Represents a 3×12 matrix whose elements are all zero.

[0069] in, ;

[0070] ; ;

[0071] The measurement noise variance matrix is ​​selected based on the GNSS position, velocity, and heading noise levels; the steering angle measurement noise variance is calculated based on the speed and vehicle wheelbase.

[0072] Specifically, the step 3 includes:

[0073] By applying Kalman filtering to the linear acceleration and curved terrain of agricultural machinery during operation, the navigation error at the corresponding moment can be obtained and the parameter error of the front wheel angular gyroscope can be estimated. By feeding the parameter error into the gyroscope output calculation process, the online calibration of the gyroscope parameters can be completed.

[0074] Example:

[0075] The simulation generates a path for agricultural machinery to travel during operation, which consists of three straight sections and two turning sections. Driving trajectory description: stop at the starting point for 100s, accelerate, turn right 90° after 100s, drive in a straight line at a constant speed, then stop for 120s, start driving in a straight line again, turn right 90° after 100s, drive in a straight line, and slow down and stop after 200s; the change of roll angle during the steering process is taken into account in trajectory generation. The noise of the output data is set to: gyro zero bias 0.3°, gyro random drift 0.01, accelerometer zero bias 50mg, random error 5mg, GNSS positioning error is set to white noise, error is 1m, height error 1m, speed error 0.5m / s. The simulation data is filtered and corrected through the above model, and the processed front wheel steering angle filter is integrated to calculate the angle, which can be obtained as follows Figure 2 As shown in the curve, after filtering, the gyro error parameter calculation is effective and the integrated angle error is reduced.

[0076] like Figure 3 As shown, the present invention also provides an online calibration device for the parameters of a MEMS gyroscope of a front wheel of an agricultural machinery, comprising the following modules:

[0077] The error differential equation establishment module establishes the error model of the front wheel gyroscope, incorporates the gyroscope's scale factor error and zero bias into the error model, where the zero bias is assumed to be a first-order Markov process, and establishes the corresponding error differential equation;

[0078] The filter construction module uses the normally functioning mainframe inertial navigation system as a reference, utilizes its navigation parameters and the front wheel gyroscope to establish a joint state equation, and uses the position and velocity information of the satellite navigation system as state constraint observations to construct a joint state Kalman filter.

[0079] The correction module improves the observability of the system through the movement of agricultural machinery during operation, uses the Kalman filter to calibrate the error parameters of the gyroscope online, and feeds the calibrated parameters back to the gyroscope output calculation process to achieve real-time online correction of the gyroscope parameters, thereby improving measurement accuracy and long-term stability.

[0080] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the steps of the above-mentioned method for online calibration of the MEMS gyroscope parameters of the front wheel of agricultural machinery are implemented.

[0081] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the steps of the above-mentioned method for online calibration of the parameters of the MEMS gyroscope of the front wheel of agricultural machinery are implemented.

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

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

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

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

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

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

Claims

1. A method for online calibration of MEMS gyroscope parameters of agricultural machinery front wheels, characterized in that: The steps include: Step 1: Establish an error model for the front wheel gyroscope, incorporate the scale factor error and zero bias of the front wheel gyroscope into the error model, where the zero bias is assumed to be a first-order Markov process, and establish the corresponding error differential equation; Step 2: Using the normally functioning main body inertial navigation system as a reference, using its navigation parameters and the front wheel gyroscope to establish a joint state equation, while taking the position and velocity information of the satellite navigation system as state constraint observations to construct a joint state Kalman filter; the joint state equation includes the navigation parameters of the main body inertial navigation system and the front wheel gyroscope, and the state variables include attitude angle error, velocity error, position error, gyroscope bias, accelerometer bias, lever arm error, wheel steering angle measurement error and front wheel gyroscope calibration coefficient error. The process noise is selected according to the noise level of the inertial device; the specific forms of the state transfer matrix and the process noise matrix are determined according to the navigation parameters of the main body inertial navigation system and the front wheel gyroscope and the motion characteristics during the agricultural machinery operation process, wherein the attitude transfer matrix is ​​calculated based on the local latitude, altitude, calculated velocity component, and accelerometer output projection; Step 3: Improve the observability of the system through the movement of agricultural machinery during operation. Use the Kalman filter to calibrate the error parameters of the front wheel gyroscope online, and feed the calibrated parameters back to the front wheel gyroscope output calculation process to achieve real-time online correction of the front wheel gyroscope parameters.

2. The online calibration method for the agricultural machinery front wheel MEMS gyroscope parameters according to claim 1 is characterized in that: In step 1, the error model of the front wheel gyroscope is expressed as: ； in, Calculate the output error for the gyro, is the gyro scale factor error, is the gyro measurement output, is the gyro zero bias; The error differential equation of the zero bias is: ; Where, is the inverse time constant coefficient, To excite white noise, Indicates gyro bias The differential of .

3. The online calibration method for the agricultural machinery front wheel MEMS gyroscope parameters according to claim 1 is characterized in that: In step 2, the measured variable y is the speed and position difference between the host inertial navigation system and the satellite navigation system, and the heading angle error of the dual-satellite navigation system, expressed as: ； in, They are the positions calculated by the satellite navigation system and the main body inertial navigation system respectively. is the lever arm vector, is the attitude angle coordinate transposed matrix, are the speeds of the strapdown solution of the satellite navigation system and the host inertial navigation system, is the carrier rotation angular velocity vector, They are the output headings calculated by the dual-antenna satellite navigation system and the main body inertial navigation system strapdown solution, are the calculated value and reference value of the front wheel angle respectively.

4. The online calibration method for the agricultural machinery front wheel MEMS gyroscope parameters according to claim 1 is characterized in that: In step 3, the error parameters of the front wheel gyroscope are calibrated online using a Kalman filter through the linear acceleration and curved driving actions during the operation of the agricultural machinery, and the calibrated parameters are fed back to the front wheel gyroscope output calculation process to achieve real-time online correction of the front wheel gyroscope parameters.

5. The online calibration method for the agricultural machinery front wheel MEMS gyroscope parameters according to claim 1 is characterized in that: In step 3, a path for the agricultural machinery to travel during operation is generated through simulation. The path consists of straight travel and turning. The driving trajectory description includes stopping, accelerating, turning, constant speed driving, and parking actions. The noise setting of the output data includes gyro bias, gyro random drift, accelerometer bias, random error, and satellite navigation system positioning error. The simulation data is filtered and corrected, and the processed front wheel angle filter is integrated to calculate the angle to verify the calibration effect.

6. An online calibration device for the parameters of a MEMS gyroscope on the front wheel of an agricultural machine, characterized in that: Includes the following modules: The error differential equation establishment module establishes the error model of the front wheel gyroscope, incorporates the scale factor error and zero bias of the front wheel gyroscope into the error model, where the zero bias is assumed to be a first-order Markov process, and establishes the corresponding error differential equation; The filter construction module uses the normally operating main body inertial navigation system as a reference, uses its navigation parameters and the front wheel gyroscope to establish a joint state equation, and uses the position and velocity information of the satellite navigation system as state constraint observations to construct a joint state Kalman filter; the joint state equation includes the navigation parameters of the main body inertial navigation system and the front wheel gyroscope, and the state variables include attitude angle error, velocity error, position error, gyroscope zero bias, accelerometer zero bias, lever arm error, wheel steering angle measurement error and front wheel gyroscope calibration coefficient error. The process noise is selected according to the noise level of the inertial device; the specific forms of the state transfer matrix and the process noise matrix are determined according to the navigation parameters of the main body inertial navigation system and the front wheel gyroscope and the motion characteristics during the operation of the agricultural machinery, among which the attitude transfer matrix is ​​calculated based on the local latitude, altitude, calculated velocity component, and accelerometer output projection; The correction module improves the observability of the system through the movement of agricultural machinery during operation, uses the Kalman filter to calibrate the error parameters of the front wheel gyroscope online, and feeds the calibrated parameters back to the front wheel gyroscope output calculation process to achieve real-time online correction of the front wheel gyroscope parameters.

7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the steps of the method for online calibration of the parameters of the agricultural machinery front wheel MEMS gyroscope are implemented as described in any one of claims 1 to 5.

8. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method for online calibration of the parameters of a MEMS gyroscope of the front wheel of an agricultural machinery as claimed in any one of claims 1 to 5 are implemented.

Images