Online calibration method and device for parameters of front wheel MEMS gyroscope of agricultural machine

Abstract

The invention provides an agricultural machinery front wheel MEMS gyroscope parameter online calibration method and device, and relates to the field of inertial measurement unit and gyroscope parameter calibration, and the method comprises the following steps: establishing an error model of a front wheel gyroscope, incorporating the scale factor error and zero offset of the gyroscope into the error model, establishing a corresponding error differential equation; the method comprises the following steps: taking a normally working host body inertial navigation system as a reference, establishing a combined state equation by using navigation parameters of the inertial navigation system and a front wheel gyroscope, and meanwhile, taking position and speed information of a satellite navigation system as state constraint observation to construct a combined state Kalman filter; the system observability is improved through movement in the agricultural machine operation process, online calibration is conducted on error parameters of the gyroscope through a Kalman filter, the calibrated parameters are fed back to the gyroscope output calculation process, and real-time online correction of the gyroscope parameters is achieved. According to the invention, measurement precision and long-term stability can be improved.

Description

Technical Field

[0001] The 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, the automated navigation and driving system of agricultural machinery is one of the key technologies to achieve precision agriculture. 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 higher and higher. The traditional manual driving method can no longer meet these requirements. The driver has a heavy workload, and repeated single driving operations increase the driver's stress and reduce work efficiency. In addition, the front wheel angle measurement system of the early automatic driving system mostly uses absolute angle sensors such as Hall effect angle sensors and encoders. These sensors require mechanical structural modifications to the front axle of the agricultural machinery during installation, and a complex calibration process is required before use. This not only consumes a lot of manpower and time, but also greatly shortens the service life of the sensor in complex and harsh operating environments.

[0003] At present, relevant personnel have studied the possibility of applying gyroscopes as angle measurement sensors to autonomous driving systems. The commonly used devices are MEMS (micro-electromechanical systems) gyroscopes. However, such sensors have 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 reduced 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 equipment, wasting a lot of time and cost. In existing gyroscope angle measurement technology, the angle is usually used as a separate system state to establish a system equation for filtering and correction. This method has poor robustness for single-variable measurement and is easily affected by the environment during field operations, resulting in abnormal or divergent measurement data. Summary of the invention

[0004] In order to solve the above 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 inertial navigation system on the main body in normal operation is used as a reference, and the navigation parameters output by the inertial navigation system are used to establish a joint state equation with the gyroscope to be calibrated installed on the front wheel. The position and speed information of the satellite navigation system is used as the state constraint observation, and a joint state Kalman filter is established to coordinately calibrate the full error parameters of the gyroscope to be calibrated, thereby simplifying the complexity and true resource occupancy 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 increases the system robustness and improves the angle measurement accuracy and reliability through speed, position and expected angle measurement input.

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

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

[0007] Step 1, establish an error model of 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 a corresponding error differential equation;

[0008] Step 2, taking the inertial navigation system of the normal working main body as a reference, establish a joint state equation by using its navigation parameters and the front-wheel gyroscope, and at the same time use the position and velocity information of the satellite navigation system as state constraint observations to construct a joint state Kalman filter;

[0009] Step 3, improve the system observability through the movement during the operation of the agricultural machine, use the Kalman filter to online calibrate the error parameters of the front-wheel gyroscope, and feedback the calibrated parameters to the output calculation process of the front-wheel gyroscope to realize real-time online correction of the parameters of the front-wheel gyroscope.

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

[0011] An error differential equation establishment module, which establishes an 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 a corresponding error differential equation;

[0012] A filter construction module, taking the inertial navigation system of the normal working main body as a reference, establishing a joint state equation by using its navigation parameters and the front-wheel gyroscope, and at the same time using the position and velocity information of the satellite navigation system as state constraint observations to construct a joint state Kalman filter;

[0013] A correction module, which improves the system observability through the movement during the operation of the agricultural machine, uses the Kalman filter to online calibrate the error parameters of the front-wheel gyroscope, and feedbacks the calibrated parameters to the output calculation process of the front-wheel gyroscope to realize real-time online correction of the parameters of the front-wheel gyroscope.

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

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

[0016] Beneficial effects:

[0017] The present invention establishes a combined state equation and an observation equation of the front-wheel gyroscope parameter model and the host body integrated navigation system. Through the measurement input of speed, position and expected rotation angle during the operation of agricultural machinery and the establishment of the combined system state equation, the scale factor error and zero bias of the front-wheel angular gyroscope are calibrated online, increasing the observability and robustness of the system, thereby improving the long-term use stability and reliability of gyro angle measurement. Description of the drawings

[0018] Figure 1 It is a flowchart of an online calibration method for the parameters of the agricultural machinery front-wheel MEMS gyroscope of the present invention.

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

[0020] Figure 3 It is a schematic diagram of an online calibration device for the parameters of the agricultural machinery front-wheel MEMS gyroscope of the present invention. Specific embodiments

[0021] In order to make the purpose, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

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

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

[0024] Step 2: Taking the normally operating host body inertial navigation system as a reference, establish a combined state equation with its navigation parameters and the front-wheel gyroscope, and at the same time use the position and speed information of the satellite navigation system as state constraint observations to construct a combined state Kalman filter;

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

[0026] Specifically, the said Step 1 includes:

[0027] Establish an error model for the front-wheel gyroscope. Since the front-wheel measurement generally uses a single-axis gyro, 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] Where, is the gyro calculation output error, is the gyro scale factor error, is the gyro measurement output, is the gyro zero bias.

[0031] Assume that the zero bias of the MEMS gyro is a first-order Markov process, then the error differential equation of the zero bias is:

[0032] ;

[0033] In the formula, is the inverse time constant coefficient, is the excitation white noise, represents the differential of the gyro zero bias .

[0034] Specifically, the said Step 2 includes:

[0035] Treat the installation error as a constant value, and establish a joint state equation for the main inertial navigation integrated navigation, the front-wheel angle measurement, and the gyro calibration coefficient according to the above error differential equation:

[0036] ;

[0037] ;

[0038] Where, is the state variable, is the dimension of the state variable, is a real number; is the state transition matrix; is the process noise matrix; is a measurement variable, is the dimension of the observable quantity; is the measurement matrix; and are the process noise and measurement noise variables respectively, and are assumed to be zero-mean Gaussian white noise, is the dimension of the process noise, represents the first derivative of the state variable x.

[0039] The state variable is selected as follows:

[0040] ;

[0041] where the intermediate variable ;

[0042] The intermediate variable ;

[0043] The intermediate variable ;

[0044] The intermediate variable ;

[0045] where, 、 and are the attitude angle errors, 、 and are the eastward, northward and upward velocity errors respectively, 、 and are the latitude error, longitude error and altitude error respectively; 、 and are the zero biases of the body's three-axis gyroscopes respectively; 、 and are the zero biases of the body's three-axis accelerometers respectively; 、 and represent the lever 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 zero bias of the gyro installed on the wheel; The superscript T represents the transpose of the matrix.

[0046] The process noise , 、 、 、 are the gyroscope random error parameters, 、 , 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 process noise matrix have the following specific forms:

[0048] , ;

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

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

[0051] , ;

[0052] ;

[0053] ;

[0054] ;

[0055] ;

[0056] ;

[0057] ;

[0058] ;

[0059] ;

[0060] where the footnotes , and represent east, north, and up directions respectively; is the local latitude; is the altitude; , , are the three components of the calculated velocity in the navigation coordinate system; , , are the projections of the accelerometer output in the navigation coordinate system. and are the principal radii of curvature along the meridian and prime vertical, respectively, is the angular velocity of the Earth's rotation;

[0061] The measurement variables of the system are the differences in velocity and position between the INS (Inertial Navigation System) and GNSS (Global Navigation Satellite System) and the heading angle error of dual GNSS, and the specific form is expressed as follows:

[0062] ;

[0063] where, are the positions obtained by the strapdown solutions of GNSS and INS, respectively, is the lever arm vector, is the attitude angle coordinate transpose matrix. are the velocities obtained by the strapdown solutions of GNSS and INS, respectively, is the carrier rotation angular velocity vector, are the output headings of dual-antenna GNSS and INS strapdown solutions, respectively, are the calculated value and reference value of the front wheel steering angle, respectively.

[0064] Observation matrix ;

[0065] where, the intermediate vector ;

[0066] Intermediate vector ;

[0067] Intermediate vector ;

[0068] where, diag() represents a diagonal matrix, is the attitude rate, represents a 3×12 matrix with elements all being 0.

[0069] where, ;

[0070] ; ;

[0071] The measurement noise variance matrix is selected according to the position, velocity and heading noise levels of GNSS; the measurement noise variance of the steering angle is calculated according to the velocity and vehicle wheelbase.

[0072] Specifically, step 3 includes:

[0073] After the straight-line acceleration, curve driving and other actions during the agricultural machinery operation are filtered by the Kalman filter, the parameter error of the front-wheel angle gyroscope can be estimated while obtaining the navigation error at the corresponding moment. Feeding back the parameter error to the gyroscope output calculation process can complete the online calibration of the gyroscope parameters.

[0074] Embodiment:

[0075] Simulate and generate a path for the agricultural machinery during operation. The path consists of three straight sections and two turns. Driving trajectory description: Stay at the starting point for 100 s, accelerate for driving, turn right by 90° after 100 s, drive straight at a constant speed, then stop for 120 s, start driving straight again, turn right by 90° after 100 s, drive straight, and decelerate and stop after 200 s; The change of the roll angle during the turning process is considered in the trajectory generation. The noise of the output data is set as follows: gyro zero bias 0.3°, gyro random drift 0.01, accelerometer zero bias 50 mg, random error 5 mg, GNSS positioning error is set as white noise, the error is 1 m, height error 1 m, speed error 0.5 m / s. Filter and correct the simulation data through the above model, and perform integral calculation on the processed front-wheel angle filter to obtain the angle. As Figure 2 shown in the curve. After filtering, the calculation of the gyro error parameters is effective, and the integral angle error is reduced.

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

[0077] Error differential equation establishment module, which establishes an error model of the front-wheel gyroscope, incorporates the scale factor error and zero bias of the 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;

[0078] Filter construction module, taking the inertial navigation system of the normal working main body as a reference, establishing a joint state equation with the front-wheel gyroscope by using its navigation parameters, and at the same time taking the position and speed information of the satellite navigation system as state constraint observations to construct a joint state Kalman filter;

[0079] Correction module, improving the system observability through the movement during the agricultural machinery operation, using the Kalman filter to perform online calibration on the error parameters of the gyroscope, feeding back the calibrated parameters to the gyroscope output calculation process, and realizing real-time online correction of the gyroscope parameters, thereby improving the measurement accuracy and long-term stability.

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

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

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

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

[0084] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory generate a manufactured article including an instruction device, and the instruction device implements in the process Figure 1 a process or multiple processes and / or blocks Figure 1 a device for the functions specified in one block or multiple blocks.

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

[0086] Although the preferred embodiments of the present invention have been described, additional changes and modifications can be made by those skilled in the art once they learn of the basic creative concept. Therefore, the appended claims are intended to be construed to include the preferred embodiments as well as all changes and modifications that fall within the scope of the present invention.

[0087] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalent technologies, the present invention is also intended to include these modifications and variations.

Claims

1. An online calibration method for the parameters of a MEMS gyroscope of the front wheel of an agricultural machine, characterized in that, It includes the following steps: 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: Taking the normally operating host body inertial navigation system as a reference, establish a joint state equation using its navigation parameters and the front-wheel gyroscope, and at the same time use the position and velocity information of the satellite navigation system as state constraint observations to construct a joint state Kalman filter; Step 3: Improve the system observability through the movement during agricultural machinery operation, use the Kalman filter to online calibrate the error parameters of the front-wheel gyroscope, and feedback the calibrated parameters to the output calculation process of the front-wheel gyroscope to achieve real-time online correction of the front-wheel gyroscope parameters.

2. The online calibration method for the parameters of the MEMS gyroscope of the front wheel of agricultural machinery according to claim 1, wherein In the above Step 1, the expression of the error model of the front-wheel gyroscope is: ； Among them, is the gyro calculation output error, is the gyro scale factor error, is the gyro measurement output, is the gyro zero bias; The error differential equation of zero bias is as follows: ; In the formula, is the inverse time constant coefficient, is the excitation white noise, represents the gyro bias and the differential of 3. The online calibration method for the parameters of the MEMS gyroscope of the front wheel of agricultural machinery according to claim 1, characterized in that, In the above Step 2, the joint state equation includes the navigation parameters of the host body inertial navigation system and the front-wheel gyroscope. 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 gyro calibration coefficient error. The process noise is selected according to the noise level of the inertial devices.

4. The online calibration method for the parameters of the MEMS gyroscope of the front wheel of agricultural machinery according to claim 1, characterized in that, In the above Step 2, the specific forms of the state transition matrix and the process noise matrix are determined according to the navigation parameters of the host body inertial navigation system and the front-wheel gyroscope and the movement characteristics during agricultural machinery operation. Among them, the attitude transition matrix is calculated based on the local latitude, altitude, calculated velocity components, and the projection of the accelerometer output.

5. The online calibration method for the parameters of the MEMS gyroscope of the front wheel of agricultural machinery according to claim 1, characterized in that, In the above Step 2, the measurement variable y is the difference in velocity and position between the host body inertial navigation system and the satellite navigation system and the course angle error of the dual-satellite navigation system, expressed as: ； Among them, are the positions obtained by the strapdown solutions of the satellite navigation system and the host inertial navigation system respectively, is the lever-arm vector, is the transpose matrix of the attitude angle coordinates, are the velocities obtained by the strapdown solutions of the satellite navigation system and the host inertial navigation system respectively, is the carrier rotational angular velocity vector, are the output headings obtained by the strapdown solutions of the dual-antenna satellite navigation system and the host inertial navigation system respectively, are the calculated value and the reference value of the front-wheel steering angle respectively.

6. The online calibration method for the parameters of the MEMS gyroscope of the agricultural machinery front wheel according to claim 1, characterized in that, In the above Step 3, through the straight-line acceleration and curve driving actions during agricultural machinery operation, use the Kalman filter to online calibrate the error parameters of the front-wheel gyroscope, and feedback the calibrated parameters to the output calculation process of the front-wheel gyroscope to achieve real-time online correction of the front-wheel gyroscope parameters.

7. The online calibration method for the parameters of the MEMS gyroscope of the front wheel of agricultural machinery according to claim 1, characterized in that In the above Step 3, generate the walking path during agricultural machinery operation through simulation. The path consists of straight lines and turns. The driving trajectory description includes stops, accelerating, turning, uniform driving, and parking actions. The noise settings of the output data include gyroscope zero bias, gyroscope random drift, accelerometer zero bias, random error, and satellite navigation system positioning error. Filter and correct the simulation data, integrate the processed front-wheel angle filter to calculate the angle, and verify the calibration effect.

8. An on-line calibration device for parameters of an MEMS gyroscope of the front wheel of an agricultural machine, characterized in that, It includes the following modules: Error differential equation establishment module: 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; Filter construction module: Taking the normally operating host body inertial navigation system as a reference, establish a joint state equation using its navigation parameters and the front-wheel gyroscope, and at the same time use the position and velocity information of the satellite navigation system as state constraint observations to construct a joint state Kalman filter; The correction module improves the system observability through the movement during the agricultural machinery operation, uses a Kalman filter to calibrate the error parameters of the front-wheel gyroscope online, and feeds the calibrated parameters back to the output calculation process of the front-wheel gyroscope to realize the real-time online correction of the front-wheel gyroscope parameters.

9. An electronic device, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, When the processor executes the program, it realizes the steps of an online calibration method for the parameters of the MEMS gyroscope of the front wheel of an agricultural machine according to any one of claims 1 to 7.

10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it realizes the steps of an online calibration method for the parameters of the MEMS gyroscope of the front wheel of an agricultural machine according to any one of claims 1 to 7.

Images