Tractor attitude detection method based on forward-forward backtracking

Abstract

The invention relates to the technical field of tractor attitude detection, in particular to a tractor attitude detection method based on forward-forward backtracking, and the method comprises the steps: firstly building a tractor attitude detection hardware system which comprises an inertial measurement unit, a Beidou satellite antenna and a microcontroller; the method comprises the following steps: constructing a strapdown inertial navigation backtracking alignment error model based on an inertial system by utilizing high-frequency acceleration and angular velocity information originally output by an inertial measurement unit, and establishing a state equation of a tractor combined attitude detection system; real-time position information of a tractor is obtained by using a Beidou satellite, a displacement vector of the tractor under an initial navigation system is solved through position difference and matrix transformation, a tractor combined attitude detection system measurement equation is constructed for filtering, a filtering result is fed back to correct an error of a strapdown inertial navigation system, and the error is calculated. Therefore, the real-time attitude of the tractor can be quickly and accurately solved. The problems that a traditional tractor attitude detection method is low in convergence speed, poor in dynamic performance and the like in a complex field operation environment are solved.

Description

Technical Field

[0001] The invention belongs to the technical field of tractor posture detection, and in particular relates to a tractor posture detection method based on forward-forward backtracking. Background Art

[0002] The soil structure and terrain undulations in the complex field environment have significant differences and unpredictability, which causes the walking posture of field operation machinery to change frequently, affecting the measurement of the motion posture of the operation machinery, and thus leading to low operation quality and efficiency. Therefore, one of the keys to realizing the intelligent and refined operation of agricultural machinery is how to accurately detect the posture of tractors and agricultural machinery in real time in complex field operation environments. Real-time and accurate measurement of the posture of agricultural machinery is the basis for realizing the precise positioning and navigation walking of agricultural machinery and the precise leveling control of wide-width machinery; it is also one of the key parameters for the kinematic and dynamic modeling of agricultural machinery and the development of agricultural vehicle safe driving warning systems. The measurement of the posture inclination of tractors and agricultural machinery is mostly obtained by inertial sensors, satellite navigation systems, and image processing. Some scholars have also studied the posture inclination monitoring based on multi-sensor fusion technology. However, in the actual complex operation environment of the tractor, a single posture solution method cannot adapt to the influence of factors such as tractor maneuvering and vibration of agricultural machinery, resulting in poor dynamic performance of tractor posture detection.

[0003] Due to the above defects of the existing tractor posture detection method, the tractor posture detection method cannot achieve the ideal effect in real sense when used, and the existing method cannot meet the needs of users. Therefore, the posture detection method proposed in this paper can effectively improve the dynamic performance of the tractor posture detection results in complex field environments and meet the actual application needs of users. Summary of the invention

[0004] In view of the above problems, the present invention proposes a tractor posture detection method based on forward-forward backtracking, aiming to solve the problem of insufficient dynamic performance of tractor field operation posture detection in complex field environments.

[0005] The above purpose is achieved through the following technical solutions:

[0006] A tractor posture detection method based on forward-forward backtracking of the present invention comprises the following steps:

[0007] S1. Build a hardware system for tractor attitude detection, including an inertial measurement unit, a Beidou satellite receiver and a microcontroller. The inertial measurement unit is installed in the tractor cab to measure the high-frequency three-axis angular velocity and acceleration information of the tractor. The inertial measurement unit is installed and fixed in the upper right front direction of the tractor; the Beidou satellite antenna is installed on the roof of the tractor, directly above the inertial measurement unit, to collect the real-time position of the tractor; the microcontroller is located in the tractor cab to receive the original data of the inertial measurement unit and the Beidou satellite position data, and perform real-time tractor attitude solution;

[0008] S2. The strapdown inertial navigation system SINS derives the inertial system error model based on forward-forward backtracking under tractor motion conditions and establishes the state equation of the tractor attitude detection system;

[0009] S3. Use Beidou satellite to obtain the tractor's motion trajectory position information in real time, solve the tractor's displacement vector in the inertial system in real time and use it as the observation quantity, so as to construct the observation equation of the tractor attitude detection system;

[0010] S4. According to the system state equation established in step S2 and the system observation equation established in step S3, a Kalman filter model is designed, and the filtering result corrects the attitude output of the strapdown inertial navigation system to improve the accuracy of the tractor attitude detection system.

[0011] Furthermore, the specific implementation of step S2 includes the following sub-steps:

[0012] S21. The carrier coordinate system of the inertial measurement unit is recorded as the carrier system b system, and the geographic coordinate system is selected as the navigation coordinate system and recorded as the navigation system n system; using the inertial system concept, the direction cosine matrix is ​​expressed as:

[0013]

[0014] in, It represents the attitude matrix with error from carrier system b to navigation system n. Represents the attitude matrix from the initial navigation system n0 to the navigation system n, It represents the attitude matrix with error from the initial carrier system b0 to the initial navigation system n0, Represents the attitude matrix with error from the carrier system b to the initial carrier system b0;

[0015] S22. Since the gyroscope measurement output contains measurement errors, the matrix with errors The direction cosine matrix differential equation is expressed as:

[0016]

[0017] in, Representation Matrix The differential form of Represents the angular velocity vector with error measured by the gyroscope;

[0018] S23. Matrix Expressed as its true value And the product of the attitude error matrix between the two:

[0019]

[0020] in, Representation Matrix and The component of the misalignment angle between is in the b0 system, and I represents the three-dimensional unit matrix;

[0021] S24. Posture matrix with error It is expressed as:

[0022]

[0023] In the formula, represents the component of the residual attitude error of the coarse alignment algorithm in the n0 system, Representation Matrix The truth value of

[0024] S25. The posture matrix with error in step S24 Converts to:

[0025]

[0026] in, Matrix representation matrix The transpose of represents the component of the residual attitude error of coarse alignment in the b0 system;

[0027] S26. After substituting steps S23 and S24 into step S21, sorting and ignoring the second-order small terms, we get:

[0028]

[0029] S27. Let the attitude error φ i It is expressed as:

[0030]

[0031] in, Representation Matrix and The component of the misalignment angle between in the n0 system;

[0032] S28. Then step S26 is re-expressed as:

[0033]

[0034] S29. In step S28, the formula is derived at both ends to obtain:

[0035]

[0036] S210. It represents the component of the residual attitude error of the rough alignment algorithm in the n0 system. The attitude error vector is a constant attitude angle error, and its differential form is:

[0037]

[0038] S211. Misalignment angle The differential expression of is:

[0039]

[0040] Among them, ε b represents the gyroscope bias vector, represents the gyroscope measurement noise vector;

[0041] S212. Combining S29, S210 and S211, the attitude error equation of the backtracking alignment can be obtained as follows:

[0042]

[0043] S213. From the specific force equation, we know:

[0044]

[0045] Among them, V n0 represents the component of the tractor speed in the initial navigation system, Represents the differential form of the velocity vector, f b is the specific force measured by the accelerometer, It represents the projection of the rotation angular velocity of the earth system relative to the inertial system in the initial navigation system. It represents the projection of the rotational angular velocity of the initial navigation system relative to the Earth system in the initial navigation system, g n represents the acceleration due to gravity,

[0046] S214. Therefore, step S213 is re-expressed as:

[0047]

[0048] S215. In the actual strapdown inertial navigation system, the specific force equation with error is expressed as:

[0049]

[0050] S216. Since the position of the underwater vehicle does not change much in a short period of time, it is considered that the attitude matrix and gravity vector g n Keeping the same, subtracting step S214 from step S215 yields:

[0051]

[0052] in, Represents vector The error form is represents the accelerometer bias vector, represents the accelerometer measurement noise vector, Indicates angular velocity The error form of

[0053] S217. By the velocity vector V n0 By integrating, we can get the motion displacement vector d of the carrier. n0 , define the displacement vector d in the n0 system n0 for:

[0054] d n0 =∫0 t V n0 dt

[0055] S218. In the actual system, the calculation expression of the displacement vector is:

[0056]

[0057] S219. Differentiate step S217 and step S218 respectively to obtain:

[0058]

[0059] S220. Subtract the two equations in step S219 to obtain:

[0060]

[0061] in, Denotes the displacement vector differential The error form is δV n0 represents the velocity vector V n0 The error form of

[0062] S221. The state vector X of the tractor posture detection system based on forward-forward backtracking under motion conditions is:

[0063]

[0064] Among them, δdn0 represents the displacement vector d n0 The error form, ε b represents the gyroscope zero bias vector, represents the accelerometer zero bias vector, and the superscript T represents the transposition operator symbol of the vector;

[0065] S222. The state equation of the tractor posture detection system based on forward-forward backtracking under motion conditions is:

[0066]

[0067] in, The differential of the state vector X, the state transfer matrix F and the system noise matrix W are expressed as:

[0068]

[0069] Furthermore, the specific implementation of step S3 includes the following sub-steps:

[0070] S31. The observation equation under the motion base is expressed as:

[0071]

[0072] Where Z represents the observed quantity, v represents the measurement noise, represents the displacement vector in the n0 system calculated from the GNSS position, represents the displacement vector in the n0 system calculated by SINS, and I represents the three-dimensional unit matrix;

[0073] S32. Since the Beidou satellite system can only obtain position information under the navigation system, the expression of the tractor displacement vector in the n0 coordinate system is derived using the real-time position of the Beidou satellite:

[0074]

[0075] Compared with the prior art, the present invention has the following advantages:

[0076] Compared with the single detection method of traditional tractor attitude detection solutions, the present invention utilizes an inertial navigation / Beidou satellite combined attitude detection solution, which can effectively solve the influence of factors such as tractor maneuverability and machine vibration on the dynamic performance of tractor attitude detection in complex field working environments, and ensure accurate real-time detection of tractor attitude.

[0077] Compared with the traditional inertial navigation / Beidou satellite combined attitude method, the present invention utilizes the idea of ​​backtracking alignment and reuses the original data of the inertial measurement unit and Beidou satellite to achieve the effect of improving the convergence speed of the tractor attitude solution. This method can take into account both the convergence speed and accuracy of the attitude solution.

[0078] Compared with other backtracking alignment methods, the tractor attitude solution based on forward-forward backtracking proposed in the present invention can store less inertial measurement units and Beidou satellite raw data to achieve tractor attitude solution, greatly freeing up microcontroller memory space, and making it easier to achieve real-time solution of tractor attitude in actual application. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 It is a schematic diagram of the tractor posture detection method of the present invention.

[0080] Figure 2 It is a flow chart of the forward-forward backtracking alignment method of the present invention.

[0081] Figure 3 This is the structural diagram of the tractor posture solution based on the IMU / Beidou combination of the present invention.

[0082] In the figure: 1. Microcontroller; 2. Inertial measurement unit; 3. Beidou satellite antenna. DETAILED DESCRIPTION

[0083] The present invention is further explained and illustrated below in conjunction with the accompanying drawings, but the protection scope of the present invention is not limited to the embodiments.

[0084] like Figure 1 As shown, a tractor posture detection method based on forward-forward backtracking of the present invention comprises the following steps:

[0085] S1. Build a hardware system for tractor attitude detection, including an inertial measurement unit, a Beidou satellite receiver and a microcontroller. The inertial measurement unit is installed in the tractor cab to measure the tractor's high-frequency three-axis angular velocity and acceleration information. The inertial measurement unit is installed and fixed in the upper right front direction of the tractor; the Beidou satellite antenna is installed on the roof of the tractor, directly above the inertial measurement unit, to collect the real-time position of the tractor; the microcontroller is located in the tractor cab to receive the original data of the inertial measurement unit and the Beidou satellite position data, and to perform real-time solution of the tractor attitude.

[0086] S2. The strapdown inertial navigation system SINS derives the inertial system error model based on forward-forward backtracking under tractor motion conditions and establishes the state equation of the tractor attitude detection system. The specific implementation method includes the following sub-steps:

[0087] S21. The carrier coordinate system of the inertial measurement unit is recorded as the carrier system b system, and the geographic coordinate system is selected as the navigation coordinate system and recorded as the navigation system n system; using the inertial system concept, the direction cosine matrix is ​​expressed as:

[0088]

[0089] in, It represents the attitude matrix with error from carrier system b to navigation system n. Represents the attitude matrix from the initial navigation system n0 to the navigation system n, It represents the attitude matrix with error from the initial carrier system b0 to the initial navigation system n0, Represents the attitude matrix with error from the carrier system b to the initial carrier system b0;

[0090] S22. Since the gyroscope measurement output contains measurement errors, the matrix with errors The direction cosine matrix differential equation is expressed as:

[0091]

[0092] in, Representation Matrix The differential form of Represents the angular velocity vector with error measured by the gyroscope;

[0093] S23. Matrix Expressed as its true value And the product of the attitude error matrix between the two:

[0094]

[0095] in, Representation Matrix and The component of the misalignment angle between is in the b0 system, I represents the three-dimensional unit matrix;

[0096] S24. Posture matrix with error It is expressed as:

[0097]

[0098] In the formula, represents the component of the residual attitude error of the coarse alignment algorithm in the n0 system, Representation Matrix The truth value of

[0099] S25. The posture matrix with error in step S24 Converts to:

[0100]

[0101] in, Matrix representation matrix The transpose of represents the component of the residual attitude error of coarse alignment in the b0 system;

[0102] S26. After substituting steps S23 and S24 into step S21, sorting and ignoring the second-order small terms, we get:

[0103]

[0104] S27. Let the attitude error φ i It is expressed as:

[0105]

[0106] in, Representation Matrix and The component of the misalignment angle between in the n0 system;

[0107] S28. Then step S26 is re-expressed as:

[0108]

[0109] S29. In step S28, the formula is derived at both ends to obtain:

[0110]

[0111] S210. It represents the component of the residual attitude error of the rough alignment algorithm in the n0 system. The attitude error vector is a constant attitude angle error, and its differential form is:

[0112]

[0113] S211. Misalignment angle The differential expression of is:

[0114]

[0115] Among them, ε b represents the gyroscope bias vector, represents the gyroscope measurement noise vector;

[0116] S212. Combining S29, S210 and S211, the attitude error equation of the backtracking alignment can be obtained as follows:

[0117]

[0118] S213. From the specific force equation, we know:

[0119]

[0120] Among them, V n0 represents the component of the tractor speed in the initial navigation system, Represents the differential form of the velocity vector, f b is the specific force measured by the accelerometer, It represents the projection of the rotation angular velocity of the earth system relative to the inertial system in the initial navigation system. It represents the projection of the rotational angular velocity of the initial navigation system relative to the Earth system in the initial navigation system, g n represents the acceleration due to gravity,

[0121] S214. Therefore, step S213 is re-expressed as:

[0122]

[0123] S215. In the actual strapdown inertial navigation system, the specific force equation with error is expressed as:

[0124]

[0125] S216. Since the position of the underwater vehicle does not change much in a short period of time, it is considered that the attitude matrix and gravity vector g n Keeping the same, subtracting step S214 from step S215 yields:

[0126]

[0127] in, Represents vector The error form is represents the accelerometer bias vector, represents the accelerometer measurement noise vector, Indicates angular velocity The error form of

[0128] S217. By the velocity vector V n0 By integrating, we can get the motion displacement vector d of the carrier. n0 , define the displacement vector d in the n0 system n0 for:

[0129] d n0 =∫0 t V n0 dt

[0130] S218. In the actual system, the calculation expression of the displacement vector is:

[0131]

[0132] S219. Differentiate step S217 and step S218 respectively to obtain:

[0133]

[0134] S220. Subtract the two equations in step S219 to obtain:

[0135]

[0136] in, Denotes the displacement vector differential The error form is δV n0 represents the velocity vector V n0 The error form of

[0137] S221. The state vector X of the tractor posture detection system based on forward-forward backtracking under motion conditions is:

[0138]

[0139] Among them, δd n0 represents the displacement vector d n0 The error form is ε b represents the gyroscope zero bias vector, represents the accelerometer zero bias vector, and the superscript T represents the transposition operator symbol of the vector;

[0140] S222. The state equation of the tractor posture detection system based on forward-forward backtracking under motion conditions is:

[0141]

[0142] in, The differential of the state vector X, the state transfer matrix F and the system noise matrix W are expressed as:

[0143]

[0144] S3. Use Beidou satellite to obtain the tractor's motion trajectory position information in real time, solve the tractor's displacement vector in the inertial system in real time and use it as the observation quantity, so as to construct the observation equation of the tractor attitude detection system. The specific implementation method is as follows:

[0145] S31. The observation equation under the motion base is expressed as:

[0146]

[0147] Where Z represents the observed quantity and v represents the measurement noise. represents the displacement vector in the n0 system calculated from the GNSS position, represents the displacement vector in the n0 system calculated by SINS, and I represents the three-dimensional unit matrix.

[0148] S32. Since the Beidou satellite system can only obtain position information under the navigation system, the expression of the tractor displacement vector in the n0 coordinate system is derived using the real-time position of the Beidou satellite:

[0149]

[0150] …

[0151]

[0152] S4. According to the state equation and observation equation established in step S2 and step S3, a Kalman filter model is designed, and the filtering result corrects the attitude output of the strapdown inertial navigation system to improve the accuracy of the tractor attitude detection system. The specific implementation method is as follows:

[0153] S41. Discretization of system state equations and measurement equations

[0154] X k =Φ k,k-1 X k-1 +W k-1

[0155] Z k =H k X k +V k

[0156] Where, X k is the state vector at time k, that is, the estimated vector; X k-1 is the state vector at time k-1; Z k is the measurement sequence at time k; W k-1 is the system noise at time k-1; V k is the measurement noise sequence at time k; Φ k,k-1 is the one-step state transfer matrix from time k-1 to time k; H k is the measurement matrix at time k,

[0157] S42. Calculate the optimal estimate of the state using the standard Kalman filter equation

[0158] State one-step prediction vector X k / k-1 :

[0159] X k / k-1 =Φ k,k-1 X k-1

[0160] State valuation calculation:

[0161] X k =X k / k-1 +K k (Z k -H kX k / k-1 )

[0162] Among them, K k Represents the filter gain:

[0163]

[0164] Among them, P k / k-1 Represents the one-step prediction mean square error matrix:

[0165]

[0166] P k-1 It represents the estimated mean square error equation at time k-1, and the estimated mean square error equation at time k P k The calculation is as follows:

[0167]

[0168] Among them, R k represents the measurement noise matrix at time k.

[0169] S43. Calibration SINS

[0170] The current optimal error estimate can be used to immediately correct the state quantity obtained by SINS from the measurement data each time.

[0171] The position correction can be made by simply subtracting the estimated value of the parameter from the estimated error of the inertial navigation system:

[0172]

[0173] Where, X c is the corrected state quantity, represents the parameter estimate, and X represents the state quantity.

Claims

1. A tractor posture detection method based on forward-forward backtracking, characterized in that: This method constructs an inertial system error model and uses the forward-forward backtracking alignment idea to quickly and accurately solve the tractor's real-time posture. The method includes the following steps: S1. Build a hardware system for tractor attitude detection, including an inertial measurement unit, a Beidou satellite receiver and a microcontroller. The inertial measurement unit is installed in the tractor cab to measure the high-frequency three-axis angular velocity and acceleration information of the tractor. The inertial measurement unit is installed and fixed in the upper right front direction of the tractor; the Beidou satellite antenna is installed on the roof of the tractor, directly above the inertial measurement unit, to collect the real-time position of the tractor; the microcontroller is located in the tractor cab to receive the original data of the inertial measurement unit and the Beidou satellite position data, and perform real-time tractor attitude solution; S2. The strapdown inertial navigation system SINS derives the inertial system error model based on forward-forward backtracking under tractor motion conditions and establishes the state equation of the tractor attitude detection system; S3. Use Beidou satellite to obtain the tractor's motion trajectory position information in real time, solve the tractor's displacement vector in the inertial system in real time and use it as the observation quantity, so as to construct the observation equation of the tractor attitude detection system; S4. According to the system state equation established in step S2 and the system observation equation established in step S3, a Kalman filter model is designed, and the filtering result corrects the attitude output of the strapdown inertial navigation system to improve the accuracy of the tractor attitude detection system.

2. The tractor posture detection method based on forward-forward backtracking according to claim 1 is characterized in that: The specific implementation of step S2 includes the following sub-steps: S21. The carrier coordinate system of the inertial measurement unit is recorded as the carrier system b system, and the geographic coordinate system is selected as the navigation coordinate system and recorded as the navigation system n system; using the inertial system concept, the direction cosine matrix is ​​expressed as: in, It represents the attitude matrix with error from carrier system b to navigation system n. Represents the attitude matrix from the initial navigation system n0 to the navigation system n, It represents the attitude matrix with error from the initial carrier system b0 to the initial navigation system n0, Represents the attitude matrix with error from the carrier system b to the initial carrier system b0; S22. Since the gyroscope measurement output contains measurement errors, the matrix with errors The direction cosine matrix differential equation is expressed as: in, Representation Matrix The differential form of Represents the angular velocity vector with error measured by the gyroscope; S23. Matrix Expressed as its true value And the product of the attitude error matrix between the two: in, Representation Matrix and The component of the misalignment angle between is in the b0 system, and I represents the three-dimensional unit matrix; S24. Posture matrix with error It is expressed as: In the formula, represents the component of the residual attitude error of the coarse alignment algorithm in the n0 system, Representation Matrix The truth value of S25. The posture matrix with error in step S24 Converts to: in, Matrix representation matrix The transpose of represents the component of the residual attitude error of coarse alignment in the b0 system; S26. After substituting steps S23 and S24 into step S21, sorting and ignoring the second-order small terms, we get: S27. Let the attitude error φ i It is expressed as: in, Representation Matrix and The component of the misalignment angle between in the n0 system; S28. Step S26 is then re-expressed as: S29. In step S28, the formula is derived at both ends to obtain: S210. It represents the component of the residual attitude error of the rough alignment algorithm in the n0 system. The attitude error vector is a constant attitude angle error, and its differential form is: S211. Misalignment angle The differential expression of is: Among them, ε b represents the gyroscope bias vector, represents the gyroscope measurement noise vector; S212. Combining S29, S210 and S211, the attitude error equation of the backtracking alignment can be obtained as follows: S213. From the specific force equation, we know: Among them, V n0 represents the component of the tractor speed in the initial navigation system, Represents the differential form of the velocity vector, f b is the specific force measured by the accelerometer, It represents the projection of the rotation angular velocity of the earth system relative to the inertial system in the initial navigation system. represents the projection of the rotational angular velocity of the initial navigation system relative to the Earth system in the initial navigation system, g n represents the acceleration due to gravity, S214. Therefore, step S213 is re-expressed as: S215. In the actual strapdown inertial navigation system, the specific force equation with error is expressed as: S216. Since the position of the underwater vehicle does not change much in a short period of time, it is considered that the attitude matrix and gravity vector g n Keeping the same, subtracting step S214 from step S215 yields: in, Represents vector The error form is represents the accelerometer bias vector, represents the accelerometer measurement noise vector, Indicates angular velocity The error form of S217. By the velocity vector V n0 By integrating, we can get the motion displacement vector d of the carrier. n0 , define the displacement vector d in the n0 system n0 for: S218. In the actual system, the calculation expression of the displacement vector is: S219. Differentiate step S217 and step S218 respectively to obtain: S220. Subtract the two equations in step S219 to obtain: in, Denotes the displacement vector differential The error form is δV n0 represents the velocity vector V n0 The error form of S221. The state vector X of the tractor posture detection system based on forward-forward backtracking under motion conditions is: Among them, δd n0 represents the displacement vector d n0 The error form is ε b represents the gyroscope zero bias vector, represents the accelerometer zero bias vector, and the superscript T represents the transposition operator symbol of the vector; S222. The state equation of the tractor posture detection system based on forward-forward backtracking under motion conditions is: in, The differential of the state vector X, the state transfer matrix F and the system noise matrix W are expressed as:

3. The tractor posture detection method based on forward-forward backtracking according to claim 2 is characterized in that: The specific implementation of step S3 includes the following sub-steps: S31. The observation equation under the motion base is expressed as: Where Z represents the observed quantity, v represents the measurement noise, represents the displacement vector in the n0 system calculated from the GNSS position, represents the displacement vector in the n0 system calculated by SINS, and I represents the three-dimensional unit matrix; S32. Since the Beidou satellite system can only obtain position information under the navigation system, the expression of the tractor displacement vector in the n0 coordinate system is derived using the real-time position of the Beidou satellite:

Images