1. When Attitude Is Wrong, Autonomous Guidance Cannot Hold the Line
With the deepening development of precision agriculture, autonomous guidance operation systems have become one of the standard configurations of modern agricultural machinery, widely applied with good results, from auto-steering navigation systems to implement control. But agricultural machinery autonomous guidance operation systems still have many problems in actual use that await deeper study, and accurate attitude measurement under complex operating conditions is one of them.
The attitude information of paddy field working machinery comes from a three-axis gyroscope and a three-axis accelerometer, so the output characteristics of the attitude measurement IMU will affect the attitude measurement and working performance of agricultural machinery in practical applications. In actual operation, three working conditions are most likely to distort the attitude angle: gyroscope drift during idle stops, external acceleration during turns, and pose transients of wide-span implements.
2. Idle Stops: Gyroscope Drift Makes the Attitude Angle Wander Further and Further
2.1 A Ten-Minute Seedling-Refill Stop Can Let Attitude Wander Ten Degrees
Cotton seeders equipped with an autonomous guidance system often need to idle-stop during operation, to replace drip tape and film or to add cotton seed, taking 5 to 20 min depending on operator proficiency. At such times the gyroscope drift characteristic causes cumulative error in attitude measurement, affecting the guidance system’s projection correction for the cotton seeder, and when operation restarts there is often a small oscillation that affects working performance. The same problem exists for rice transplanters equipped with an autonomous guidance system when they idle-stop to replenish seedlings. The gyroscope drift characteristic refers to the random fluctuation of the output during gyroscope operation, and mainly includes static drift, dynamic drift and random drift.
2.2 Test Data: Six Degrees of Wander in Five Minutes
In the test with a non-zero roll angle, the cumulative roll angle error grows roughly with time, reaching 6° within 5 min and 11.87° within 15 min; the roll angle measured by the three-axis accelerometer, though it fluctuates slightly during the idle stop, stays within ±0.8°, and the effect it brings is far smaller than the cumulative tilt error caused by gyroscope drift, so it can serve as a substitute method. The test result with a non-zero pitch angle is similar, with a cumulative pitch angle error of 6.78° within 5 min and 15.68° within 15 min; the pitch angle measured by the three-axis accelerometer fluctuates by ±0.9°. So when the transplanter is judged by a threshold to be in the seedling-refill idle state, the system attitude angle can be estimated from the three-axis accelerometer measurements while the gyroscope measurements are ignored, avoiding the effect of the gyroscope drift characteristic on attitude measurement.
2.3 The Fix: Identify the Idle State and Hand Attitude Over to the Accelerometer
The gyroscope drift test verified that a rice transplanter idling to replenish seedlings suffers cumulative attitude measurement error, with 5 min causing no less than 6° of cumulative attitude angle error; in theory this produces no less than 0.1 m of positioning error, which in turn causes small oscillation in the actual working path and affects working performance. By setting a reasonable three-axis resultant acceleration threshold, using only the three-axis accelerometer to estimate the attitude angle while the rice transplanter idles to replenish seedlings avoids the effect of gyroscope drift. The threshold range for the three-axis resultant acceleration is 0.8 g to 1.25 g, with a program time window of 20 s (ensuring the cumulative error does not exceed ±0.4°, meeting the requirement of autonomous guidance operation).
Under normal working conditions, the complementary filter uses the three-axis accelerometer information to compensate and correct the gyroscope drift error. The gyroscope solves the attitude angle by integrating angular rate over a short time, and over long use produces cumulative error because of its drift characteristic; the accelerometer has slower dynamic characteristics and readily introduces high-frequency or impulse noise, but has no cumulative error, and can correct the gyroscope’s attitude angle solution — the two are complementary in the frequency domain, satisfying the complementary filter requirement. The four-element (quaternion) attitude solving method is used together with the complementary filter to produce the attitude output.
2.4 Field Verification: Two Minutes of Idling, Error Clearly Pressed Down
The vehicle idle-stop identification algorithm accurately recognizes the transplanter’s idle stop for seedling replenishment; a temporary stop of more than 20 s can be identified accurately. Under an idle stop of just over 2 min, compared with the gyroscope-integration attitude estimation method, the roll angle can be reduced by up to 0.8° of cumulative error and the pitch angle by up to 0.45° of cumulative error. Under normal transplanting operation, the algorithm measures attitude accurately and smoothly; the attitude angle solved by gyroscope integration, though smooth in output, has relatively large measurement error under working conditions, with the roll angle maximum error approaching 2° and the pitch angle maximum error approaching 1.5°.
3. Headland Turns: External Acceleration Leaks into Gravity and Pushes the Attitude Angle Off
3.1 “Gravity Equals Attitude” Only Holds Quasi-Statically — Turning Breaks That Premise
When paddy field working machinery is quasi-static, the accelerometer output is affected only by gravity and the resultant acceleration is the gravitational acceleration, and at that time the three-axis accelerometer can accurately estimate the attitude tilt. But in actual operation the machine is affected by external forces and is non-quasi-static, and especially during vehicle turning the accelerometer is affected by external forces more markedly. Without a suitable model or additional external sensors, it is very difficult to distinguish external force acceleration from gravitational acceleration, which seriously affects the subsequent attitude angle fusion. To verify the effect of external acceleration on attitude measurement, a parallel slide rail was first used in the laboratory for testing.
3.2 Slide Rail Test: Horizontal Acceleration Hurts Attitude Most
During the test, the theoretical values of roll and pitch attitude angles should both be zero. With an external acceleration of ±5 m/s² applied on the accelerometer X sensitive axis, the pitch angle measurement error reaches ±10° while the effect on roll angle measurement is negligible in comparison. With an external acceleration of ±7 m/s² applied on the Y sensitive axis, the roll angle measurement error reaches ±10° while the effect on pitch angle measurement is smaller. With an external acceleration of ±6 m/s² applied on the Z sensitive axis, the effect on roll and pitch attitude angle measurement is small, with an average error less than ±0.5°.
3.3 The Fix: Build a First-Order Model of External Acceleration and Feed It into an Extended Kalman Filter
Paddy field working machinery is affected by horizontal external acceleration in actual operation, most markedly during turns. Therefore a model must be built from the external acceleration characteristics and identified, compensated and corrected in real time during operation. With the direction cosine method used for attitude solving and a Kalman filter fusion algorithm designed on a first-order external accelerometer model, external acceleration identification and accurate attitude estimation can be achieved for the field working vehicle under dynamic conditions. A first-order low-pass filter measurement model is built for external acceleration, in which k is a constant coefficient between 0 and 1 that depends on the cutoff frequency of the filter. Because acceleration and external acceleration are independent of each other, the system’s observation noise covariance matrix is obtained by adding the measurement error covariance matrix of the external acceleration model and the measurement noise covariance matrix of the three-axis accelerometer. Under quasi-static conditions (no external acceleration), the covariance matrix compensates for the effect of external acceleration on attitude measurement by increasing the observation noise, and plays a key role. Once the final measurement matrix estimate is solved, the external acceleration value at the current instant can be computed, and the roll and pitch attitude angles then solved from it.
3.4 Verification: Turntable Calibrates Accuracy, Laser Grader Runs Real Conditions
To verify the accuracy of the external acceleration identification algorithm, a three-axis multi-function turntable was used for testing, with the outer frame rotating at angular rates ranging from ±20 °/s to ±200 °/s. When the outer frame angular rate is below ±200 °/s, the algorithm’s maximum error in estimating external acceleration is 0.2554 g, with an average error not exceeding 0.1439 g.
A paddy field laser grader used for land leveling and grade control was then used as the platform to verify the algorithm’s accuracy in attitude measurement when external acceleration is present. A conventional attitude measurement system produces a following fluctuation of about 1° during turning, affecting the grader’s working performance; the external acceleration identification and attitude synchronous estimation algorithm, even under pronounced external acceleration, still measures the grader’s attitude stably and smoothly. Taking the conventional attitude measurement system as reference, during straight-line operation of the grader the roll angle measurement has an average error of 0.21° and a maximum error of 0.83°, and the pitch angle an average error of 0.23° and a maximum error of 0.66°; during curve turning the roll angle measurement has an average error of 0.38° and a maximum error of 0.95°, and the pitch angle an average error of 0.41° and a maximum error of 1.04°.
4. Wide-Span Machinery Pose Transients: An Instant of Sway Can Ruin a Pass
4.1 Wide-Span Implements Sway Fast, and Conventional Fusion Cannot Keep Up
For high-clearance wide-span agricultural machinery under complex working conditions, the attitude during operation is easily affected by the field environment and undergoes transients, with instantaneous sway that seriously affects working quality. A MEMS micro-inertial sensor is an ideal wide-bandwidth attitude measurement device; if an adaptive attitude fusion model is built from its sensor operating characteristics, the problem of inaccurate attitude measurement caused by pose transients of wide-span agricultural machinery can be well solved, improving both working efficiency and working quality.
4.2 The Fix: Let the Kalman Filter’s Noise Covariance Follow the Working Condition
An adaptive Kalman filter attitude fusion algorithm is proposed based on the operating characteristics of the MEMS gyroscope and accelerometer. An adaptive filter relies on a recursive algorithm, so that it can complete the filtering satisfactorily when complete knowledge of the relevant signal characteristics is unavailable, and is therefore suited to measuring the attitude of field wide-span agricultural machinery affected by instantaneous sway. In the Kalman filter, the high-clearance wide-span sprayer changes attitude quickly during paddy field operation, especially when transient sway occurs, so attitude measurement requires a high sampling frequency, and at that time the gyroscope measurement information is the main attitude solving information. The state vector of the Kalman filter therefore contains not only the roll and pitch attitude angles but also the X-axis and Y-axis gyroscope measurement biases, which are compensated and corrected while the filter runs.
In the adaptive part, the paddy field working machinery attitude measurement system has dynamic time-varying characteristics, and both the state noise and the observation noise follow the dynamic changes of the system. To improve the fusion accuracy of the attitude measurement system under pose transients, the system’s state noise covariance matrix and observation noise covariance matrix must be adaptively adjusted according to the system’s motion characteristics. For a MEMS micro-inertial sensor, the gyroscope bandwidth has a measurement range limit, so the system state noise and the magnitude of the system’s rotational motion can be approximately treated as linear, and the state noise covariance is interpolated between the system’s maximum and minimum state noise covariances according to the magnitude of the rotational motion; the measurement noise of the attitude measurement system has time-varying characteristics that depend on the system’s observation state, and the external acceleration generated during actual operation increases the system measurement noise.
4.3 Verification: About Twice the Accuracy of a Conventional Kalman Filter
With the paddy field high-clearance wide-span sprayer as the platform and a dual-satellite-antenna positioning and attitude module as the reference, a field test was carried out. Taking the module’s heading output as reference, the roll attitude angle measurement deviations of the adaptive Kalman filter attitude fusion algorithm and of a Kalman filter fusion algorithm without adaptation were compared. The test result shows that, compared with the conventional Kalman filter attitude fusion algorithm, the Kalman filter attitude fusion algorithm with the adaptive process has higher measurement accuracy, about twice that of the conventional Kalman filter algorithm (±0.5°); the measurement is more stable and the measurement deviation follows a normal distribution, with more accurate and more stable attitude measurement (no small spikes). The adaptive Kalman filter attitude fusion algorithm achieves an average attitude measurement error of 0.55° in actual sprayer operation and a maximum measurement error of less than 0.91°, meeting the precision operation requirements of wide-span high-clearance agricultural machinery.
5. Putting the Three Problems Together: The Error Is Pressed Down by Division of Labor
5.1 Before Data Enters Fusion, Remove Outliers and Smooth It
Among the many outlier identification methods, the Hampel filter is regarded as the most stable and most effective outlier identification method; it improves on the median filter and has 2 adjustable parameters: the moving window width N and the scale factor t. To eliminate the error introduced by outlier replacement, a weighted moving average filter algorithm is used to guarantee the fidelity of outlier reconstruction while smoothing the sensor information. After correction by the combined Hampel filter and weighted moving average filter algorithm, all outliers were identified and effectively replaced, and the small spikes were also well smoothed.
5.2 The Attitude Angle Initial Value Determines How Fast the System Converges
The convergence speed of the system is one of the key parameters for judging the effectiveness and stability of an attitude measurement system. Besides the attitude fusion algorithm, the convergence speed is mainly affected by the attitude initial value; if the initial value assignment error is too large, the system oscillates and cannot converge. For a MEMS accelerometer, when it is in the quasi-static state (at rest or with no external acceleration) it senses only gravitational acceleration. When paddy field working machinery goes from system startup to actual operation it must pass through parameter setting, adjustment and calibration, and warm-up, during which the attitude measurement system is quasi-static (vehicle vibration does not affect the accelerometer measurement output), so the three-axis accelerometer information can be used to measure the system’s attitude angle initial value.
5.3 Summary
The error sources differ, and so does the division of labor among the algorithms. During idle stops the problem lies in the accumulation of gyroscope drift: the fix is to identify the idle state, hand attitude over to the accelerometer, and fuse the two with a complementary filter. During turns the problem lies in external acceleration leaking into gravity: the fix is to build a first-order model of the external acceleration and embed it in an extended Kalman filter, estimating the attitude angle and the external acceleration together. When wide-span machinery undergoes pose transients the problem lies in the system noise varying with the working condition: the fix is to let the Kalman filter’s state noise and observation noise covariances adapt. All three use a three-axis gyroscope plus a three-axis accelerometer as the sensing basis, without adding external sensors.
For agricultural machinery attitude measurement, autonomous guidance and working quality control, DAISCH offers IMU core modules, inertial intelligent sensors and integrated GNSS/INS (RTK-INS) navigation as product forms, on which these attitude fusion algorithms can be engineered.
