Agricultural machine chassis posture dynamic estimation system based on multi-sensor fusion

Abstract

The application relates to the technical field of agricultural machinery control, and particularly discloses a dynamic estimation system for the posture of an agricultural machine chassis based on multi-sensor fusion. The system comprises a sensor array, a preprocessing and synchronization module, a dual-rate layered fusion estimation module, and a posture output and diagnosis module. Through a layered asynchronous fusion architecture of high-rate posture prediction and low-rate measurement update, and by adopting a simplified filtering algorithm with pre-calculated steady-state gain, high-precision and low-delay dynamic estimation of the chassis posture is realized on a resource-limited embedded controller, and the system has the ability of system state diagnosis and degraded operation. The system decouples the calculation-intensive state update and the lightweight posture prediction in the time domain by designing a dual-rate layered fusion estimation architecture. The simplified iterative Kalman filtering framework adopted by the system reduces the online calculation complexity from the cubic level of the traditional extended Kalman filter to the linear level by calculating and solidifying the steady-state gain matrix offline.

Description

Technical Field

[0001] This invention belongs to the field of agricultural machinery control technology, specifically relating to a dynamic estimation system for the attitude of agricultural machinery chassis based on multi-sensor fusion. Background Technology

[0002] In the field of agricultural machinery automation and intelligence, high-precision and high-reliability operation control is the foundation for achieving precision agriculture. Dynamic perception of the chassis attitude of agricultural machinery is a prerequisite for ensuring the realization of advanced functions such as automatic driving, path tracking, and operation quality. Its accuracy directly affects the handling stability and operation precision of agricultural machinery.

[0003] Dynamic estimation of chassis attitude based on multi-sensor fusion is currently the mainstream technology for achieving this sensing function. This technology aims to integrate data from heterogeneous sensors such as inertial measurement units, global navigation satellite systems, and wheel speed encoders, and use state estimation algorithms to calculate the attitude parameters of agricultural machinery in real time, such as roll angle, pitch angle, yaw angle, and angular velocity, providing state feedback to the upper-level control system.

[0004] Existing technologies typically employ extended Kalman filtering or its improved algorithms as multi-sensor fusion algorithms. However, the implementation of such algorithms on embedded computing platforms suffers from the following problems: complex matrix operations and iterative update processes introduce non-negligible computational delays, making it difficult to meet the real-time requirements for attitude information update frequency when agricultural machinery performs rapid steering control in complex field terrain.

[0005] To reduce computational burden, some existing lightweight solutions often improve processing speed by simplifying the system state vector or linearizing the model. However, this approach sacrifices the accuracy and robustness of attitude estimation. Especially when agricultural machinery encounters severe bumps or sensors experience brief anomalies, the estimation error will increase, making it impossible to provide a basis for control decisions.

[0006] Therefore, how to achieve both high precision and low latency in dynamic chassis attitude estimation within resource-constrained agricultural machinery embedded systems has become an urgent technical challenge. Summary of the Invention

[0007] The purpose of this invention is to provide a dynamic attitude estimation system for agricultural machinery chassis based on multi-sensor fusion, so as to solve the technical contradiction between high accuracy and low latency of attitude estimation in existing technologies on embedded platforms.

[0008] The technical solution of the present invention is a dynamic estimation system for the attitude of agricultural machinery chassis based on multi-sensor fusion. The system is deployed in an embedded controller of agricultural machinery and includes a sensor array, a preprocessing and synchronization module, a dual-rate hierarchical fusion estimation module, and an attitude output and diagnosis module.

[0009] A sensor array is used to acquire multi-source heterogeneous motion data of the agricultural machinery chassis in real time. It includes a three-axis microelectromechanical system (MEMS) inertial measurement unit (IMSU), a dual-antenna global navigation satellite system (GNSS) receiver, and at least two wheel speed encoders mounted on the non-drive wheels. The IMS outputs raw data containing three-axis angular velocities and three-axis specific forces at a first fixed frequency. The dual-antenna GNSS receiver outputs information including position, velocity, and heading angle calculated from the dual-antenna baseline at a second fixed frequency. The wheel speed encoders output the rotation pulse counts corresponding to the wheels at a third fixed frequency.

[0010] The preprocessing and synchronization module is connected to the sensor array to perform time alignment, invalid outlier removal, and preliminary calibration on the raw data from each sensor, generating a standardized data stream with unified timestamps. This module incorporates a high-precision hardware timer, using an integer multiple of the system's highest sampling frequency as the reference clock to hardware-level timestamp all input data streams. For inertial measurement unit (IMU) data, the module performs online compensation for zero bias and scaling factors. For GNSS (Global Navigation Satellite System) data, the module performs carrier phase smoothing pseudorange processing to improve positioning accuracy and calculates a high-precision heading angle based on dual-antenna phase center differential. For wheel speed encoder data, the module converts pulse counts into linear velocity based on the effective wheel radius.

[0011] The dual-rate hierarchical fusion estimation module receives a standard data stream from the preprocessing and synchronization modules and performs attitude calculation using a hierarchical asynchronous fusion architecture. This dual-rate hierarchical fusion estimation module further includes a high-rate attitude prediction submodule, a low-rate measurement update submodule, and a state buffer and interpolator.

[0012] The high-speed attitude prediction submodule operates independently at a high update frequency, consistent with the sampling frequency of the inertial measurement unit (IMU). This submodule uses only the three-axis angular velocities output by the IMU as input, employing quaternion-based attitude kinematics equations for recursive calculation to predict the agricultural machinery's attitude quaternions in real time. Its calculation process completely avoids complex matrix inversion operations, involving only quaternion multiplication and normalization, thus ensuring low single-step calculation latency. This high-speed attitude prediction submodule continuously outputs high-frequency attitude prediction sequences.

[0013] The low-rate measurement update submodule operates at a lower update frequency, matching the effective data update frequency of the Global Navigation Satellite System (GNSS) and wheel speed encoder. At the arrival of each low-rate cycle, this submodule performs a state correction based on multi-source measurements. Internally, it maintains an extended system state vector, which includes not only attitude quaternions but also gyro bias from the inertial measurement unit (IMU), accelerometer bias, and GNSS velocity error.

[0014] In each update cycle, the low-rate measurement update submodule performs the following steps: It obtains the time-aligned high-rate attitude prediction value corresponding to the current moment from the state buffer and interpolator as the state prediction value. It constructs a multi-source measurement model, specifically including: projecting the gravity vector onto the body coordinate system based on the current attitude prediction value, and combining it with the specific force vector measured by the inertial measurement unit to form an inclination measurement model; constructing a heading angle measurement model based on the heading angle calculated by the dual-antenna global navigation satellite system; and constructing a velocity measurement model based on the vehicle's lateral and longitudinal velocities calculated by the wheel speed encoder, combined with the current attitude prediction value after coordinate rotation, and the velocity from the global navigation satellite system.

[0015] A simplified iterative Kalman filter framework is employed for state updates. This framework fixes the state transition matrix as a linearized approximation of the Jacobian matrix and pre-calculates its steady-state gain matrix. During each update, only one calculation of the measurement residual and correction of the state vector are performed, significantly reducing the computational load of online Jacobian matrix calculation and covariance matrix updates in traditional extended Kalman filters. Finally, the updated system state vector, particularly the corrected attitude quaternions and the estimated sensor error, is fed back to the state buffer and interpolator, and used to reset the recursive initial values ​​of the high-speed attitude prediction submodule to correct prediction drift caused by gyroscope bias accumulation.

[0016] The state buffer and interpolator cache the continuous attitude sequence output by the high-rate attitude prediction submodule. After the low-rate measurement update submodule completes a state correction, the interpolator performs linear or spherical linear interpolation on the cached high-rate attitude sequence based on the timestamp of the correction, generating a smooth attitude trajectory from past correction points to the current correction point. This trajectory is immediately used to overwrite the original cache, ensuring the continuity of the high-rate output. Simultaneously, it provides the latest corrected attitude as a new starting point to the high-rate attitude prediction submodule.

[0017] The attitude output and diagnostic module is connected to the dual-rate hierarchical fusion estimation module. This module reads the interpolated high-frequency attitude sequence from the state buffer and interpolator in real time and converts it into engineering-usable roll, pitch, and yaw angle outputs. Furthermore, it continuously monitors the filtered prediction bias sequence in the low-rate measurement update submodule and the data health status of each sensor. By calculating the autocorrelation and chi-square test statistic of the prediction bias sequence, the module can diagnose in real time whether the fusion filter is in optimal operating condition. Upon detecting sensor failure or model mismatch, it triggers degradation strategies, such as switching to pure inertial navigation mode or increasing the process noise covariance of the filter.

[0018] In one embodiment of the present invention, the steady-state gain matrix of the simplified iterative Kalman filter framework is obtained through offline calculation. Specifically, before system deployment, based on the typical motion model of agricultural machinery and sensor noise characteristics, the complete extended Kalman filter algorithm is run in a simulation environment until its state estimation error covariance matrix converges to a steady state. The steady-state gain matrix under this steady state is extracted and stored in the non-volatile memory of the embedded controller. During online operation, the low-rate measurement update submodule directly calls this pre-stored steady-state gain matrix and multiplies it by the current measurement residual to complete the state correction.

[0019] Furthermore, the spherical linear interpolation algorithm used by the state buffer and interpolator is optimized for quaternion representation. Based on the attitude quaternions corresponding to the two consecutive low-rate correction moments, it calculates the relative rotation quaternion, and evenly divides the rotation into several parts corresponding to the high-rate period. Through quaternion exponentiation and multiplication, it generates a smooth sequence of attitude quaternions for intermediate moments, thereby ensuring the geometric shortest path characteristic of the interpolated attitude change.

[0020] Furthermore, the degradation strategy of the attitude output and diagnostic module includes a multi-level response. When only a brief loss of the Global Navigation Satellite System signal is detected, the system automatically ignores the heading angle and velocity measurement models, and the state update relies solely on the tilt angle measurement model and wheel speed model. When a wheel speed encoder anomaly is further detected, the system relies entirely on the inertial measurement unit (IMU) and historical steady-state gain for state prediction and maintenance. When a severe anomaly in the IMU data is diagnosed, the system outputs the last valid fusion estimation result and issues an alarm.

[0021] Furthermore, the system runs on a multi-tasking real-time operating system. The high-rate attitude prediction submodule is given the highest task priority and is bound to an independent hardware timer interrupt service routine for execution to ensure the strict periodicity of its output frequency. The low-rate measurement update submodule and the attitude output and diagnostic module are executed sequentially under the scheduling of the operating system as lower-priority tasks, and exchange data with the high-rate submodule through shared memory.

[0022] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0023] 1. This invention decouples computationally intensive state updates from lightweight attitude prediction in the time domain by designing a dual-rate hierarchical fusion estimation architecture. The high-rate attitude prediction submodule performs only simple quaternion recursion, providing high-frequency attitude signals that meet the requirements of rapid control with extremely low latency; the low-rate measurement update submodule integrates multi-source measurement information at a lower frequency and uses pre-computed steady-state gains for efficient correction, fundamentally solving the real-time bottleneck caused by complex matrix operations. This architecture enables the system to simultaneously achieve millisecond-level high-frequency output and centimeter-level and angle-level high-precision estimation on resource-constrained embedded platforms.

[0024] 2. The simplified iterative Kalman filter framework adopted in this invention reduces the online computational complexity from the cubic level of the traditional extended Kalman filter to the linear level by calculating and fixing the steady-state gain matrix offline. During online operation, only vector and matrix multiplication operations are required, greatly reducing the consumption of microprocessor computing resources, improving the reliability and determinism of the system, and enabling it to run stably on low-cost agricultural machinery embedded controllers.

[0025] 3. This invention cleverly bridges the time and rate differences between high- and low-rate data streams through a state buffer and interpolator module. The state buffer and interpolator module not only ensures the continuity and smoothness of high-frequency attitude output within the low-rate correction interval, but more importantly, it achieves online compensation for historical prediction errors through interpolation backtracking and sequence overlay mechanisms. This ensures that the attitude information output at any given time incorporates the latest available global measurement information, thereby significantly improving the system's estimation accuracy and response speed in dynamically changing environments.

[0026] 4. The attitude output and diagnostic module integrated in this invention provides system-level health management capabilities. By monitoring filtered information and sensor status in real time, the system can autonomously identify abnormal operating conditions and execute corresponding degradation strategies, enhancing robustness under complex electromagnetic environments and mechanical vibration interference in the field. This ensures that even when the performance of some sensors temporarily degrades, the system can still provide usable, progressively degraded attitude information, avoiding sudden control failures and improving the safety and reliability of intelligent agricultural machinery operations. Attached Figure Description

[0027] Figure 1 This is a schematic diagram of the overall technical solution architecture of the agricultural machinery chassis attitude dynamic estimation system based on multi-sensor fusion proposed in this invention;

[0028] Figure 2 This is a schematic diagram of the core principle framework of the dual-rate hierarchical fusion estimation module in this invention;

[0029] Figure 3 This is a logical flow diagram of the preprocessing and synchronization module and the sensor array in this invention. Detailed Implementation

[0030] Example 1: This invention provides a dynamic attitude estimation system for agricultural machinery chassis based on multi-sensor fusion. This system is deployed in an embedded controller of the agricultural machinery, aiming to solve the problem of embedded platforms struggling to balance high accuracy and low latency in attitude estimation. Please refer to the appendix. Figure 1 To be continued Figure 3 The system consists of a sensor array, a preprocessing and synchronization module, a dual-rate hierarchical fusion estimation module, and an attitude output and diagnostic module. The system runs in a multi-tasking real-time operating system environment. Different modules have different task priorities based on their computational characteristics and real-time requirements, and exchange data through shared memory and semaphore mechanisms.

[0031] The sensor array serves as the direct interface for the system to perceive the external physical world, acquiring multi-source heterogeneous motion data of the agricultural machinery chassis in three-dimensional space in real time. Specifically, the sensor array includes a three-axis microelectromechanical system (MEMS) inertial measurement unit (IMSU), a dual-antenna global navigation satellite system (GNSS) receiver, and at least two wheel speed encoders mounted on the non-drive wheels. The IMS is mounted near the center of gravity of the agricultural machinery chassis and integrates a three-axis gyroscope and a three-axis accelerometer. The gyroscope outputs raw angular velocity data of the agricultural machinery chassis around the X, Y, and Z axes of the machine's coordinate system at a first fixed frequency, in radians per second. The accelerometer outputs raw specific force data along the X, Y, and Z axes of the machine's coordinate system at the same frequency, in meters per second squared.

[0032] The first fixed frequency is selected based on control requirements, typically ranging from 100 Hz to 200 Hz. The two antennas of the dual-antenna global navigation satellite system receiver are installed along the longitudinal axis of the agricultural machinery, forming a fixed baseline of known length. The receiver outputs information at a second fixed frequency, including three-dimensional position coordinates, a three-dimensional velocity vector, and a heading angle calculated using dual-antenna carrier phase differential technology.

[0033] The second fixed frequency is limited by the satellite signal update rate, typically ranging from 1 Hz to 10 Hz. The wheel speed encoders are mounted on the hubs or drive shafts of the left and right front non-drive wheels, and each encoder outputs a pulse count corresponding to the wheel's rotation at a third fixed frequency.

[0034] The preprocessing and synchronization module calculates the wheel angular velocity by counting the number of pulses within a fixed sampling period. The module then converts the wheel angular velocity into linear velocity based on the effective wheel radius.

[0035] The preprocessing and synchronization module receives all raw data streams from the sensor array and transforms them into a standardized data stream with a unified time base and reliable quality. This module integrates a high-precision hardware timer that uses an integer multiple of the highest sampling frequency in the system (i.e., a first fixed frequency) as its reference clock source.

[0036] For each frame of raw sensor data that arrives, including the angular velocity and specific force of the inertial measurement unit, the position, velocity, and heading angle data packets from the global navigation satellite system receiver, and the pulse edge interrupt signal from the wheel speed encoder, the preprocessing and synchronization module adds a precise timestamp based on this reference clock at the hardware level, marking the time when the data was generated. This hardware-level timestamp mechanism is a prerequisite for achieving subsequent time alignment of multi-sensor data.

[0037] After timestamping, the preprocessing and synchronization module performs targeted preprocessing algorithms on various types of data. For the inertial measurement unit (IMU) data stream, this module performs online compensation. The compensation process first reads the static zero bias constant value obtained by the IMU during the calibration phase from the non-volatile memory and subtracts the constant value from the current raw data.

[0038] The preprocessing and synchronization module dynamically corrects the zero bias by consulting a pre-stored table of zero-bias temperature compensation coefficients based on temperature sensor readings. For scaling factor nonlinearity errors, the module compensates in real-time using a second-order polynomial fitting model. The long-term stability and accuracy of the compensated angular velocity and specific force data are significantly improved. For the GNSS data stream, the preprocessing and synchronization module performs carrier phase smoothing pseudorange processing. The preprocessing algorithm leverages the low noise but integer ambiguity of carrier phase observations, combining them with pseudorange observations. A recursive smoothing filter within a time window effectively suppresses high-frequency multipath noise and thermal noise in the pseudorange observations, thereby improving the accuracy of the position solution, especially in the elevation direction.

[0039] The preprocessing and synchronization module, based on the raw carrier phase observations received from dual antennas, eliminates receiver clock errors and satellite clock errors through double-difference processing, calculating a high-precision heading angle in radians with an accuracy of 0.1 degrees. For the wheel speed encoder data stream, the preprocessing and synchronization module first converts the captured pulse counts into the angular increment of wheel rotation.

[0040] The calibrated effective radius of the corresponding wheel is read from the parameter configuration table. The effective radius takes into account the effects of tire load and air pressure changes. By multiplying the angular increment by the effective radius, the preprocessing and synchronization module calculates the average linear velocity of the wheel during the measurement cycle. In addition, the preprocessing and synchronization module has built-in invalid outlier removal logic. By checking whether the rate of change between consecutive data points exceeds the physical motion limit threshold, such as the wheel linear acceleration exceeding 10 m / s², it identifies and discards abnormal data points caused by pulse count loss or interference, and fills them with the valid value from the previous moment or through linear prediction.

[0041] The dual-rate hierarchical fusion estimation module employs an innovative hierarchical asynchronous fusion architecture, decoupling high-frequency attitude prediction from low-frequency multi-source measurement updates in the time domain. This achieves a balance between high accuracy and high real-time performance on resource-constrained embedded platforms. Please refer to the attached document. Figure 2 The dual-rate hierarchical fusion estimation module is further subdivided into three functional sub-units: a high-rate attitude prediction sub-module, a low-rate measurement update sub-module, and a state buffer and interpolator. These three sub-units work together to form a dynamic closed-loop estimation system.

[0042] The high-rate attitude prediction submodule operates independently at a high update frequency, which is strictly consistent with the sampling frequency of the inertial measurement unit (IMU), i.e., the first fixed frequency, for example, 100 Hz. This high-rate attitude prediction submodule is given the highest task priority in the entire system and is bound to an independent hardware timer interrupt service routine for execution, ensuring high determinism and extremely low jitter in its execution cycle. Its input is only the three-axis angular velocity data after preprocessing and compensation by the synchronization module. The core algorithm of this high-rate attitude prediction submodule is based on the recursive calculation of the attitude kinematics equations using quaternions. A quaternion is a four-dimensional hypercomplex number used to represent three-dimensional rotation, defined as... , For the scalar part, , , This is the vector part.

[0043] During system initialization, the initial attitude quaternion is formed by the initial heading angle provided by the Global Navigation Satellite System and the initial roll and pitch angles calculated from the gravity vector sensed by the accelerometer under static conditions. In each high-rate cycle, assuming from... Time's up The time interval is The components of the body's angular velocity on the three axes are obtained as follows: First, construct the angular increment quaternion. Its calculation involves only multiplication of angular velocity and time interval. Subsequently, the attitude quaternions are updated via quaternion multiplication: . The quaternion before the update, This represents quaternion multiplication. Finally, the updated quaternion... A normalization operation is performed to ensure that the magnitude is 1. The entire calculation process completely avoids the complex trigonometric function operations or matrix inversion operations required by Euler angles or direction cosine matrices, and only includes basic quaternion multiplication and normalization operations. The single-step calculation latency is typically on the order of microseconds. This high-speed attitude prediction submodule continuously outputs a high-frequency attitude quaternion prediction sequence, denoted as... .

[0044] The low-rate measurement update submodule operates at a low update frequency, matching the effective data update frequency of the Global Navigation Satellite System and wheel speed encoders, for example, 10 Hz. This low-rate measurement update submodule is executed periodically as a low-priority task, scheduled by the operating system. Internally, it maintains an extended system state vector. This vector not only contains the quaternion error state representing the attitude, but also... (Three-dimensional small-angle vector), also including the gyroscope zero-bias error of the inertial measurement unit. Accelerometer zero bias error and Global Navigation Satellite System velocity error At the arrival of each low-rate update cycle, the low-rate measurement update submodule performs a state correction based on multi-source measurements.

[0045] The specific steps include: First, the submodule sends a request to the state buffer and interpolator to obtain the time-aligned state corresponding to the current update time. High-speed attitude prediction ,Should This will serve as the prediction baseline for this state update. Secondly, a multi-source measurement model is constructed. Measurement model one is the tilt measurement model: using the current attitude prediction value to determine the reference gravity vector in the navigation coordinate system. ( Transform the gravitational acceleration (into the body coordinate system) to obtain the theoretical specific force. The theoretical force ratio is compared with the force vector measured by the inertial measurement unit and after zero-bias compensation. The difference between the two values ​​constitutes the tilt angle measurement residual.

[0046] Measurement Model 2 is a heading angle measurement model: based on high-speed attitude prediction values. Extracting the yaw angle The high-precision heading angle calculated by the dual-antenna global navigation satellite system The difference is directly calculated to form the residual for the heading angle measurement. Measurement model three is the speed measurement model: the wheel linear velocity is calculated using the left front and right front wheel speed encoders. and Based on the known track width and wheelbase, the longitudinal velocity at the vehicle center is calculated using the Ackermann steering geometry model. With lateral velocity Then, using the current attitude prediction value, the velocity in the body coordinate system is... Rotate to the navigation coordinate system to obtain the navigation system velocity calculated from the wheel velocity. Compare this speed with the navigation speed provided by the Global Navigation Satellite System. The difference between the two values ​​constitutes the velocity measurement residual.

[0047] A simplified iterative Kalman filter framework is employed for state updates. The innovation of this framework lies in replacing the online real-time computation of the state transition matrix, Jacobian matrix, and steady-state gain matrix in traditional extended Kalman filtering with a pre-computed and fixed steady-state gain matrix that is calculated offline. The offline computation process is completed before system deployment: In the simulation environment, based on the typical kinematic model of the agricultural machinery and the noise characteristic parameters provided by the sensor manufacturer, the complete extended Kalman filter algorithm is run. By simulating the movement of the agricultural machinery in typical operating scenarios such as straight-line driving, turning, and climbing, the state estimation error covariance matrix of the filter is determined. Iterate and update until convergence to a steady-state value. Extract the steady-state gain matrix corresponding to this steady state. This data is then stored as a constant matrix in the flash memory of the embedded controller. During online operation, the low-rate measurement update submodule first combines the aforementioned multi-source measurement residuals into a measurement residual vector within each cycle. Then, the pre-stored steady-state gain matrix is ​​directly called. Perform a state correction calculation once: .in, This is the estimated correction amount for the system state vector error. The calculation involves only matrix and vector multiplication operations, with a computational complexity of linear order. Finally, the correction amount... Used to update the system state: for quaternion error states The attitude quaternion is updated through exponential mapping; for the sensor zero bias error, it is directly accumulated into the estimated zero bias value.

[0048] The state buffer and interpolator internally maintain a circular buffer to continuously cache the latest several cycles of attitude quaternion prediction sequences output by the high-rate attitude prediction submodule. After the low-rate measurement update submodule completes a state correction, it will generate the corrected optimal attitude quaternion. And the correction timestamp corresponding to the status correction. Upon receiving this information, the state buffer and interpolator immediately initiate the interpolation backtracking and overwrite process.

[0049] The interpolation backtracking and overwrite process first uses the correction timestamp as the basis. Locate the two high-rate prediction points with the closest timestamps in the circular buffer. and Assuming two high-rate prediction points and The corresponding postures are respectively and ,and Next, a spherical linear interpolation algorithm for quaternions is used to calculate... The interpolated pose at time step 1. The spherical linear interpolation algorithm guarantees that the interpolation path is the shortest arc on the quaternion hypersphere, thus avoiding pose singularities and non-uniform rotations. The algorithm first calculates the interpolated pose from... arrive Relative rotation quaternions Then, based on the time scaling factor... ,calculate of Powers. Interpolation orientation quaternions are generated through quaternion exponentiation and multiplication. Theoretically, this interpolation result should be consistent with... Very close, but the significance of the interpolation process lies in generating a timestamp from the last state correction. Up to the corrected timestamp Smooth attitude trajectory at all high-speed moments in between.

[0050] The state buffer and interpolator utilize the optimal pose quaternion after the previous state correction. and the corrected optimal pose quaternion The system recalculates all cached high-rate prediction sequences between these two time points using spherical linear interpolation, completely overwriting the corresponding segments in the original buffer with the new interpolated sequence. This mechanism is crucial, ensuring that the high-frequency attitude history information output by the system at any given time incorporates the latest global measurement corrections, achieving online compensation for prediction drift. Finally, the state buffer and interpolator convert the latest corrected attitude quaternions... The updated sensor bias estimate is sent to the high-speed attitude prediction submodule as a new recursive initial condition to reset its recursive process, thereby closing the loop to correct the long-term prediction error caused by the accumulation of gyroscope bias.

[0051] The attitude output and diagnostic module serves as the interface between the system and external applications such as automatic steering control systems and suspension leveling systems. This module reads the interpolated and optimized high-frequency attitude quaternion sequence in real time from the state buffer and interpolator. For each quaternion, the attitude output and diagnostic module executes a standard conversion algorithm to decompose it into engineering-intuitive Euler angles: roll angle. Pitch angle and yaw angle The conversion process involves calculating the quaternion elements into a direction cosine matrix, extracting Euler angles from the direction cosine matrix, and handling singularity cases. The converted Euler angles are output at a high frequency with low delay, meeting the requirements of fast control loops.

[0052] The attitude output and diagnostic module continuously monitors signals in the low-rate measurement update submodule. The first is the filtered information sequence, i.e., the measurement residual before each state update. Under the assumption of optimal filter operation, the information sequence should be zero-mean white noise. The attitude output and diagnostic module calculates the autocorrelation function of the information sequence in real time and checks whether it has a peak at zero delay and is close to zero at other delays to determine whether the filter effectively utilizes the measurement information.

[0053] The module calculates the normalized sum of squares of the prediction bias, which is the inverse weighted statistic of the prediction bias covariance matrix. This statistic theoretically follows a chi-square distribution. Through the chi-square test, the module can diagnose whether the measurement model matches the actual situation or whether there are unmodeled interferences. Secondly, the module monitors the data health status of each sensor: it checks the positioning accuracy factor, signal-to-noise ratio, and number of satellites output by the global navigation satellite system receiver to determine signal quality; it checks the continuity of the wheel speed encoder pulse count to determine if pulse loss or jamming has occurred; and it checks whether the inertial measurement unit data exceeds its range or whether the output remains unchanged.

[0054] Based on the diagnostic information above, the attitude output and diagnostic module executes a multi-level degradation strategy to ensure system robustness.

[0055] In the first stage, when a temporary blockage of the Global Navigation Satellite System (GNSS) signal causes the accuracy factor to exceed the limit or the number of satellites to be insufficient, but the wheel speed encoder and inertial measurement unit are functioning normally, the module notifies the low-rate measurement update submodule to temporarily disable the GNSS velocity component in the heading angle measurement model and velocity measurement model during this update cycle. Status updates rely solely on the tilt angle measurement model and wheel speed model; system accuracy decreases slightly but still allows for continued operation.

[0056] In the second stage, if an anomaly is detected in a wheel speed encoder signal, the module disables the entire speed measurement model. The system relies entirely on the inertial measurement unit and historical steady-state gain for state prediction and maintenance. Attitude estimation will drift over time, but it will remain usable in the short term.

[0057] At the third level, when a severe anomaly is detected in the inertial measurement unit (IMU) data, such as output saturation or a surge in variance, the module determines that the core sensor has failed. At this point, the system stops updating and continues to output the last valid fusion estimation result. Simultaneously, it issues the highest-level audible and visual alarm to the driver via the controller area network bus or an alarm, indicating the need for manual intervention. This gradual degradation strategy avoids abrupt changes in output, providing a buffer time for the upper-level control system.

[0058] The data flow and control flow of the entire system strictly follow the appendix. Figure 1 To be continued Figure 3 The architecture is shown. Raw data generated by the sensor array is normalized by a preprocessing and synchronization module, forming a standard data stream that is input into the dual-rate hierarchical fusion estimation module. Within the dual-rate hierarchical fusion estimation module, the high-rate attitude prediction submodule and the low-rate measurement update submodule operate asynchronously in parallel, coupled and error-compensated through a state buffer and interpolator, ultimately generating high-precision and high-frequency attitude information. The attitude output and diagnostic module handles the conversion of the final value and monitors the system state, forming a complete, reliable, and real-time closed loop from perception and estimation to output.

[0059] Example 2: Based on the system architecture described in Example 1, this example elaborates on the offline calculation and optimization deployment process of the steady-state gain matrix in the simplified iterative Kalman filter framework, and elaborates on an enhanced sensor fault isolation and reconfiguration strategy to further improve the system's survivability under extreme conditions.

[0060] steady-state gain matrix Offline computation is a systematic engineering process, and its accuracy directly determines the final performance limit of the online fusion filter. The computation process begins with the construction of a high-fidelity simulation environment. The simulation environment needs to accurately model the kinematic and dynamic characteristics of the agricultural machinery chassis, including vehicle mass, moment of inertia, suspension stiffness, and damping parameters. The motion scenario library needs to cover all typical operating modes of intelligent agricultural machinery: high-speed straight-line travel in the field, low-speed turning at the edge of the field, slope operation, pitch and roll vibrations caused by crossing furrows, and switching between loaded and unloaded states. The sensor noise model must be parameterized based on the datasheets of the specific inertial measurement unit, GNSS receiver, and wheel speed encoder model to be selected. Noise types include gyroscope angular random walk, zero-bias instability, accelerometer and zero-bias repeatability, white noise and time-related noise of GNSS position and velocity, and quantization noise and pulse jitter of wheel speed encoder.

[0061] In the simulation, the complete extended Kalman filter algorithm is run. The state vector of this extended Kalman filter algorithm is consistent with that defined in Example 1. The process noise covariance matrix Q is set according to the sensor noise parameters and the uncertainty of the motion model. The measurement noise covariance matrix R is dynamically adjusted according to the measured accuracy of each sensor under different operating conditions. For example, when the simulated global navigation satellite system signal is affected by multipath, the variance values ​​of the corresponding position and velocity measurement components in matrix R are increased accordingly.

[0062] The filter performs prediction updates at a relatively high frequency, such as 100 Hz, and measurement updates at the global navigation satellite system's update frequency, such as 10 Hz. After running simulations for hours or even tens of hours, the diagonal elements of the state estimation error covariance matrix P, i.e., the estimated variances of each state component, are observed. When these variance values ​​no longer monotonically decrease with time but fluctuate slightly around the steady-state value, the filter is considered to have converged to a steady state. At this point, the steady-state gain matrix is ​​recorded. The value at each measurement update time. Since the system is time-varying, the gain matrix also exhibits periodic small fluctuations in steady state.

Claims

1. A dynamic estimation system for the attitude of agricultural machinery chassis based on multi-sensor fusion, characterized in that, Deployed in agricultural machinery embedded controllers, including: Sensor arrays are used to collect multi-source heterogeneous motion data of agricultural machinery chassis in real time; The preprocessing and synchronization module, connected to the sensor array, is used to perform time alignment, invalid outlier removal and preliminary calibration on the raw data from each sensor, and generate a standard data stream with unified timestamps. The dual-rate hierarchical fusion estimation module is connected to the preprocessing and synchronization module and is used to receive the standard data stream and perform attitude calculation using a hierarchical asynchronous fusion architecture. The dual-rate hierarchical fusion estimation module includes a high-rate attitude prediction submodule, a low-rate measurement update submodule, and a state buffer and interpolator. The attitude output and diagnosis module is connected to the dual-rate hierarchical fusion estimation module. It is used to read the interpolated high-frequency attitude sequence from the state buffer and interpolator in real time and convert it into roll angle, pitch angle and yaw angle output. The sensor array includes a three-axis microelectromechanical system inertial measurement unit, a dual-antenna global navigation satellite system receiver, and at least two wheel speed encoders mounted on the non-drive wheels; The triaxial microelectromechanical system inertial measurement unit outputs raw data containing triaxial angular velocity and triaxial specific force at a first fixed frequency; The dual-antenna global navigation satellite system receiver outputs information including position, velocity, and heading angle calculated from the dual-antenna baseline at a second fixed frequency. The wheel speed encoder outputs a rotation pulse count corresponding to the wheel at a third fixed frequency; The high-speed attitude prediction submodule operates independently at a high update frequency consistent with the first fixed frequency. It takes only the three-axis angular velocity output by the inertial measurement unit as input, and uses the attitude kinematic equation based on quaternions for recursive calculation to predict the attitude quaternions of the agricultural machinery in real time, and continuously outputs a high-frequency attitude prediction sequence. The low-rate measurement update submodule operates at a low update frequency that matches the second fixed frequency and the third fixed frequency. Internally, it maintains an extended system state vector, which includes attitude quaternions, gyroscope zero bias of the inertial measurement unit, accelerometer zero bias, and global navigation satellite system velocity error. The low-rate measurement update submodule performs a state correction based on multi-source measurement once in each low-rate cycle. The process includes: obtaining the time-aligned attitude prediction value corresponding to the current moment from the state buffer and interpolator as the state prediction value. Construct a multi-source measurement model that includes a tilt angle measurement model, a heading angle measurement model, and a velocity measurement model; A simplified iterative Kalman filter framework is used for state updates. The framework fixes the state transition matrix as a linearized approximation of the Jacobian matrix and pre-calculates its steady-state gain matrix. Measurement residual calculation and state vector correction are performed once during each update. The updated system state vector is fed back to the state buffer and interpolator and used to reset the recursive initial value of the high-rate attitude prediction submodule. The state buffer and interpolator are used to cache the continuous attitude sequence output by the high-rate attitude prediction submodule. After the low-rate measurement update submodule completes a state correction, the cached high-rate attitude sequence is interpolated according to the timestamp of the correction time to generate an attitude trajectory that smoothly transitions from the past correction point to the current correction point. This trajectory is used to overwrite the original cache, and the latest corrected attitude is provided as a new starting point to the high-rate attitude prediction submodule.

2. The agricultural machinery chassis attitude dynamic estimation system based on multi-sensor fusion according to claim 1, characterized in that, The preprocessing and synchronization module has a built-in high-precision hardware timer that uses an integer multiple of the system’s highest sampling frequency as the reference clock to timestamp all input data streams at the hardware level. For inertial measurement unit data, the preprocessing and synchronization module performs online compensation for zero bias and scaling factor; For Global Navigation Satellite System data, the preprocessing and synchronization module performs carrier phase smoothing pseudorange processing to improve positioning accuracy, and calculates high-precision heading angles based on dual-antenna phase center differential. For wheel speed encoder data, the preprocessing and synchronization module converts pulse counts into linear velocity based on the effective radius of the wheel.

3. The agricultural machinery chassis attitude dynamic estimation system based on multi-sensor fusion according to claim 1, characterized in that, The attitude output and diagnosis module also continuously monitors the filter information sequence in the low-rate measurement update submodule and the data health status of each sensor. It diagnoses the working status of the fusion filter in real time by calculating the autocorrelation and chi-square test statistic of the information sequence, and triggers a degradation strategy when sensor failure or model mismatch is detected.

4. The agricultural machinery chassis attitude dynamic estimation system based on multi-sensor fusion according to claim 1, characterized in that, The steady-state gain matrix of the simplified iterative Kalman filter framework was obtained through offline calculation; The specific process is as follows: Before system deployment, based on the typical motion model of agricultural machinery and sensor noise characteristics, the complete extended Kalman filter algorithm is run in the simulation environment until its state estimation error covariance matrix converges to a steady state. Extract the steady-state gain matrix under this steady state and store it in the non-volatile memory of the embedded controller; When running online, the low-rate measurement update submodule directly calls this pre-stored steady-state gain matrix and multiplies it with the current measurement residual to complete the state correction.

5. The agricultural machinery chassis attitude dynamic estimation system based on multi-sensor fusion according to claim 1, characterized in that, The state buffer and interpolator use a spherical linear interpolation algorithm optimized for quaternion representation. The spherical linear interpolation algorithm calculates the relative rotation quaternion based on the attitude quaternion corresponding to the two low-rate correction moments, and divides the rotation evenly into several parts corresponding to the high-rate period. Through quaternion exponentiation and multiplication, a smooth attitude quaternion sequence at the intermediate moment is generated.

6. The agricultural machinery chassis attitude dynamic estimation system based on multi-sensor fusion according to claim 1, characterized in that, The attitude output and the degradation strategy triggered by the diagnostic module include multi-level responses; When only a brief loss of the Global Navigation Satellite System signal is detected, the system automatically ignores the heading angle and speed measurement models, and the state update relies solely on the tilt angle measurement model and wheel speed model. When a wheel speed encoder anomaly is detected, state prediction and maintenance rely entirely on the inertial measurement unit and historical steady-state gain. When a serious anomaly is diagnosed in the inertial measurement unit data, the system outputs the last valid fusion estimation result and issues an alarm.

7. The agricultural machinery chassis attitude dynamic estimation system based on multi-sensor fusion according to claim 1, characterized in that, The system runs on a multi-tasking real-time operating system; The high-speed attitude prediction submodule is given the highest task priority and is bound to an independent hardware timer interrupt service routine for execution. The low-rate measurement update submodule and the attitude output and diagnosis module are executed sequentially under the scheduling of the operating system as lower-priority tasks, and exchange data with the high-rate attitude prediction submodule through shared memory.

8. The agricultural machinery chassis attitude dynamic estimation system based on multi-sensor fusion according to claim 1, characterized in that, The construction process of the tilt angle measurement model is as follows: Based on the current attitude prediction value, the gravity vector is projected onto the body coordinate system to obtain the theoretical specific force vector. The theoretical specific force vector is compared with the specific force vector measured by the inertial measurement unit, and the difference constitutes the tilt angle measurement residual.

Images