Tractor sideslip angle real-time estimation system and method based on auxiliary navigation

Abstract

The invention discloses a tractor sideslip angle real-time estimation system and method based on auxiliary navigation, and belongs to the technical field of agricultural machinery intelligent navigation and motion control. The method comprises the steps of obtaining a vehicle motion state vector; constructing an extended kinematics model and a two-degree-of-freedom dynamical model based on the vehicle motion state vector; obtaining the initial state of the tire sideslip angle based on an extended kinematics model and an extended Kalman filtering algorithm; on the basis of a two-degree-of-freedom dynamic model and a Luenberger state observer, carrying out online identification and correction on the initial state of the tire sideslip angle to obtain corrected estimated values of the front and rear tire sideslip angles; and obtaining a fusion weighting factor of the sideslip angles of the front and rear wheels based on the corrected estimated values of the sideslip angles of the front and rear tires, the vehicle speed and the vehicle steering angle, and obtaining a final estimated value of the sideslip angles of the front and rear wheels based on the fusion weighting factor. According to the method, the perception robustness of the navigation system to the vehicle sideslip state can be remarkably improved.

Description

Technical Field

[0001] This invention belongs to the field of intelligent navigation and motion control technology for agricultural machinery, specifically relating to a real-time estimation system and method for the sideslip angle of a tractor based on assisted navigation. Background Technology

[0002] With the rapid development of precision agriculture technology, agricultural machinery equipped with automatic navigation systems (such as tractors) has been widely used in all stages of operations, including plowing, planting, management, and harvesting. In the automatic navigation control closed loop, the sideslip angle, as a key state parameter characterizing the angle between the vehicle's actual motion vector and the vehicle's longitudinal axis, is a necessary feedback variable for achieving precise path tracking and attitude control, and determines the lateral correction capability of the path tracking controller.

[0003] Existing agricultural navigation systems mostly employ a loose / tight coupling approach between GNSS (Global Navigation Satellite System) and IMU (Inertial Measurement Unit) for state estimation. However, the unstructured environment of farmland is characterized by large terrain undulations, soft and slippery soil, and time-varying adhesion coefficients, making agricultural machinery highly susceptible to nonlinear sideslip. Traditional estimation algorithms based on kinematic models or single dynamic models often suffer from estimation divergence and response lag under low-speed, heavy-load, or slight sideslip conditions due to insufficient observation, making it difficult to meet the real-time state feedback requirements of high-precision navigation.

[0004] Furthermore, while sideslip angle measurement schemes based on external environmental perception sensors such as vision and lidar are theoretically feasible, their system architecture is complex and hardware costs are high. More importantly, field operations are often accompanied by high dust levels, drastic changes in light, and crop shaking, which severely affect the signal-to-noise ratio and robustness of such optical sensors, limiting their large-scale application in agricultural production.

[0005] In conclusion, developing a real-time, high-precision estimation method for tractor sideslip angle that does not require expensive additional sensors and can adapt to complex, time-varying field conditions is of significant engineering importance for improving the anti-disturbance capability and operational quality of agricultural machinery automatic navigation systems. Summary of the Invention

[0006] This invention aims to solve the technical problems of insufficient sideslip angle estimation accuracy, lag in dynamic response, and poor adaptability to unstructured field environments in existing agricultural machinery automatic navigation systems, and provides a real-time estimation method for the sideslip angle of tractors traveling in straight lines in the field based on the fusion of dynamic and kinematic models.

[0007] This invention, relying on a high-precision vehicle-mounted navigation hardware platform, constructs a multi-model fusion framework incorporating vehicle kinematics and dynamics characteristics. Combining sensor data for state observation, and building a multi-source information fusion observer, it enables real-time, high-precision estimation of the vehicle's sideslip state in complex field environments. This method significantly improves the robustness of the navigation system's perception of vehicle sideslip state, providing accurate state feedback to the path tracking controller. This effectively enhances the control stability and straight-line tracking accuracy of automatic navigation, ensuring consistent quality and efficiency in automated field operations.

[0008] To achieve the above objectives, the present invention provides the following solution: a real-time estimation system for the sideslip angle of a tractor based on assisted navigation, comprising: The vehicle motion state vector acquisition module is used to acquire multi-source motion state information of the tractor in real time based on the tractor auxiliary navigation system, and process the multi-source motion state information to obtain the vehicle motion state vector; the auxiliary navigation system includes: a dual-antenna GNSS positioning unit, an IMU, a main controller, a host computer, a power management module, and a software module; The model building module is used to construct an extended kinematic model and a two-degree-of-freedom dynamic model based on the vehicle's motion state vector. An initial state estimation module is used to obtain the initial state of the tire sideslip angle based on the extended kinematics model and the extended Kalman filter algorithm. The correction module is used to identify and correct the initial state of the tire sideslip angle online based on the two-degree-of-freedom dynamic model and the Lumberjack state observer, so as to obtain the corrected estimated values ​​of the front and rear tire sideslip angles. The fusion weighting module is used to obtain a fusion weighting factor for the front and rear wheel sideslip angles based on the corrected front and rear tire sideslip angle estimates, vehicle speed, and vehicle steering angle, and to obtain the final front and rear wheel sideslip angle estimates based on the fusion weighting factor.

[0009] More preferably, the process by which the vehicle motion state vector acquisition module obtains the vehicle motion state vector includes: The dual-antenna GNSS positioning unit is used to acquire the real-time global position coordinates and heading information of the tractor to establish a navigation reference; the front wheel steering angle is obtained using the front wheel IMU. Use vehicle body IMU to monitor vehicle body yaw rate The system captures the vehicle's dynamic posture; it then performs digital filtering and noise reduction, timestamp synchronization and alignment, and extrinsic parameter calibration based on the installation location on the obtained raw data to obtain a spatiotemporally unified vehicle motion state vector.

[0010] More preferably, the extended kinematic model includes: ; In the formula, This represents the lateral velocity of the vehicle relative to the reference path. Indicates the longitudinal speed of the tractor; Indicates heading deviation; This represents the estimated final front wheel sideslip angle; This represents the estimated final rear wheel sideslip angle; This indicates the wheelbase of the vehicle.

[0011] More preferably, the two-degree-of-freedom dynamic model includes: ; ; In the formula, Indicates yaw acceleration; a , b These represent the front wheelbase and the rear wheelbase, respectively. Indicates the sideslip angle of the center of mass; Indicates the overall vehicle weight; Indicates the longitudinal velocity of the vehicle's center of gravity; Indicates the vehicle's steering angle; Indicates tire lateral stiffness; It represents the moment of inertia.

[0012] More preferably, the process by which the initial state estimation module obtains the initial state of the tire sideslip angle includes: The extended kinematic model is expressed as a state equation, and the state vector is chosen as... The control input is For nonlinear functions Regarding the state vector Find the partial derivatives and calculate the time interval. State Jacobian matrix ;in, This represents the observed value of the lateral deviation; Indicates the observed heading deviation; superscript T Indicates transpose; This represents the estimated sideslip angles of the front and rear wheels; Based on the lateral deviation and heading deviation, an observation vector is constructed. Using the state equation as the system model, and based on the state Jacobian matrix, a state transition model of the extended Kalman filter algorithm is established. Based on the prediction and update steps of the extended Kalman filter algorithm, the state vector is recursively estimated, and the estimated values ​​of the front wheel sideslip angle and the rear wheel sideslip angle are obtained from the converged state vector.

[0013] More preferably, the process by which the correction module obtains the corrected estimates of the front and rear tire sideslip angles includes: First, the estimated front wheel sideslip angle and the estimated rear wheel sideslip angle... For input, based on the vehicle's front and rear wheelbase. Based on the geometric proportional relationship, the initial geometric observation value of the centroid sideslip angle is calculated; Based on the aforementioned two-degree-of-freedom dynamic model, the standard state-space update equation is obtained; The Luneburger observer structure includes: ; In the formula, The derivative of the system state estimation vector; This represents the estimated value of the system state vector; A Represents the state matrix; B Represents the input matrix; C Indicates the output matrix; H This represents the optimal state feedback gain matrix; Construct the error dynamic equation and solve for the optimal state feedback gain matrix using the linear quadratic regulator method. H ; Substituting the optimal state feedback gain matrix into the Romberg observer, and utilizing the deviation between the yaw rate measured by the sensors and the model prediction, a closed-loop correction is performed on the state vector to obtain the corrected estimates of the front and rear wheel sideslip angles. .

[0014] More preferably, the fusion weighting factor for the front wheel sideslip angle includes: ; In the formula, Indicates the front wheel speed influence factor; Indicates the front wheel steering angle influence factor; The fusion weighting factor for the rear wheel sideslip angle includes: ; In the formula, This indicates the rear wheel speed influence factor.

[0015] More preferably, the final estimated front and rear wheel sideslip angles include: This invention also provides a real-time estimation method for the sideslip angle of a tractor based on assisted navigation, comprising the following steps: S1. Based on the tractor auxiliary navigation system, multi-source motion state information of the tractor is acquired in real time, and the multi-source motion state information is processed to obtain the vehicle motion state vector; the auxiliary navigation system includes: a dual-antenna GNSS positioning unit, an IMU, a main controller, a host computer, a power management module, and a software module; S2. Construct an extended kinematic model and a two-degree-of-freedom dynamic model based on the vehicle motion state vector; S3. The initial state of the tire sideslip angle is obtained based on the extended kinematics model and the extended Kalman filter algorithm. S4. Based on the two-degree-of-freedom dynamic model and the Lumberjack state observer, the initial state of the tire sideslip angle is identified and corrected online to obtain the corrected estimated values ​​of the front and rear tire sideslip angles. S5. Based on the corrected estimated front and rear tire sideslip angles, vehicle speed, and vehicle steering angle, a fusion weighting factor for the front and rear tire sideslip angles is obtained, and the final estimated front and rear tire sideslip angles are obtained based on the fusion weighting factor.

[0016] Compared with the prior art, the beneficial effects of the present invention are as follows: According to the method of this invention, using a Dongfanghong LY1104 tractor as the experimental object, an auxiliary navigation system capable of running the real-time sideslip angle estimation algorithm provided by this invention was installed. During straight-line navigation operations in the field, the following occurred: 1. Significantly improved estimation accuracy: After adopting the sideslip angle estimation algorithm of this invention, the sideslip angle estimation error is controlled within ±0.5°. Under complex field conditions (such as slippery ground and slope operation), the estimation accuracy can still remain stable, meeting the requirements of high-precision navigation operations.

[0017] 2. Excellent real-time performance: The algorithm's single calculation time is less than 20ms, fully meeting the real-time requirements of the navigation system. Even under dynamic conditions such as high-speed steering or rapid acceleration of the tractor, the system can still output accurate sideslip angle estimates in real time, providing reliable status information for navigation control.

[0018] 3. Significantly improved navigation accuracy: After applying this sideslip angle estimation algorithm, the lateral deviation of the tractor's straight-line navigation operation considering the sideslip angle is reduced from the original ±8cm to ±3cm, a reduction of 62.5%, and the navigation accuracy reaches the centimeter level, meeting the requirements of precision agriculture operations.

[0019] In summary, the real-time sideslip angle estimation algorithm provided by this invention has high accuracy and excellent real-time performance, which can significantly improve the overall performance of agricultural machinery automatic navigation systems and provide strong technical support for precision agriculture operations. Attached Figure Description

[0020] To more clearly illustrate the technical solution of the present invention, the drawings used in the embodiments are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0021] Figure 1 This is a schematic diagram of the overall framework of the tractor sideslip angle real-time estimation system based on assisted navigation proposed in an embodiment of the present invention; Figure 2 This is a schematic diagram of the sideslip angle estimation algorithm using a Luneburg state observer, as described in an embodiment of the present invention. Figure 3 This is a schematic diagram of the adaptive weighted fusion estimation results of two observers according to an embodiment of the present invention; Figure 4 A schematic diagram of the true values ​​of the front wheel steering angle and speed provided for an embodiment of the present invention; Figure 5 This is a schematic diagram of the fusion factor calculated according to an embodiment of the present invention; Figure 6 This is a schematic diagram of the estimated front and rear wheel sideslip angles obtained in an embodiment of the present invention. Detailed Implementation

[0022] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0023] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0024] Example 1: Combined with appendix Figure 1 This embodiment provides a detailed description of a real-time tractor sideslip angle estimation system based on assisted navigation proposed in this invention.

[0025] The vehicle motion state vector acquisition module is used to acquire multi-source motion state information of the tractor in real time based on the tractor auxiliary navigation system, and process the multi-source motion state information to obtain the vehicle motion state vector.

[0026] In this embodiment, to achieve real-time acquisition and online estimation of the sideslip angle, an auxiliary navigation system is installed on the tractor. This auxiliary navigation system includes: a dual-antenna GNSS positioning unit, an IMU, a main controller, a host computer and power management module, and a software module for running the algorithm of this invention.

[0027] Dual-antenna GNSS positioning unit: Two receiving antennas are rigidly fixed to the top of the tractor cab by a crossbeam bracket. The antenna connection is parallel to the center line of the tractor's rear axle and there is no metal obstruction above. It is used to obtain high-precision geodetic coordinates (latitude / longitude / elevation) of the tractor in real time and to obtain accurate heading angle data calculated by the dual-antenna baseline.

[0028] IMUs are horizontally mounted on the tractor cab and the front wheel steering knuckle shaft, respectively, and rigidly connected using shock-absorbing pads to reduce high-frequency vibration interference from the engine. During installation, ensure that the IMU's coordinate axes are strictly aligned with the vehicle's coordinate axes (longitudinal, lateral, and vertical) or have been calibrated for external parameters. They are used for high-frequency acquisition of three-axis acceleration, three-axis angular velocity, and Euler angles (pitch and roll) attitude parameters during tractor operation.

[0029] Main controller: Installed in the cab, it serves as the core computing unit of the system. It receives and parses GNSS and IMU data via serial port, performs coordinate system transformation, runs sideslip angle fusion estimation algorithm and path tracking control algorithm, and finally drives the underlying actuators by outputting signals through the CAN bus.

[0030] Host computer and power management module: Installed in the cab, the host computer is responsible for work path planning, parameter configuration and status visualization; the power management module is used to ensure stable power supply to each vehicle electronic unit.

[0031] This invention designs a fusion estimation system for the sideslip angle of a tractor during field operations based on the aforementioned auxiliary navigation system. Based on the above, the current vehicle sideslip angle is calculated according to the current high-precision position information, IMU inertial navigation information and front wheel steering angle, and a fusion estimation is performed based on the front wheel steering angle and vehicle speed.

[0032] The main controller acquires three core data streams in parallel via serial port: First, it uses a dual-antenna GNSS positioning unit to obtain the tractor's real-time global position coordinates and heading information to establish a navigation reference; then, it uses the front wheel IMU to obtain the front wheel steering angle. Use vehicle body IMU to monitor vehicle body yaw rate The system captures the vehicle's dynamic attitude; the main controller performs digital filtering and noise reduction, timestamp synchronization and alignment, and extrinsic parameter calibration based on the installation position on the above raw data to construct a spatiotemporally unified vehicle motion state vector, which serves as the input for subsequent sideslip state estimation.

[0033] The model building module is used to construct an extended kinematic model and a two-degree-of-freedom dynamic model based on the vehicle's motion state vector.

[0034] First, a nonholonomic constraint-based extended kinematic model of a single-track tractor is constructed: based on the Ackermann steering geometry principle, a nonholonomic constraint condition is introduced where the rear axle center velocity direction coincides with the vehicle's longitudinal axis, simplifying the four-wheeled tractor into a two-wheeled single-track kinematic model. The extended kinematic model uses the tractor's longitudinal velocity... Front wheel steering angle As input, establish lateral deviation and heading deviation The differential equation is used to characterize the ideal geometric motion trajectory of a tractor on a low-speed, high-adhesion surface, providing a nominal state reference for the system in steady state for the state estimator, as shown in formula (1).

[0035] (1) In the formula, This represents the lateral velocity of the vehicle relative to the reference path. Indicates the rate of change of vehicle deviation; This represents the estimated final front wheel sideslip angle; This represents the estimated final rear wheel sideslip angle; This indicates the wheelbase of the vehicle.

[0036] To address the nonlinear disturbance caused by tire lateral slip during farmland operations, a dynamic equation incorporating yaw and lateral motion is established. The vehicle body yaw rate is selected as the optimal value. and the sideslip angle of the center of mass The state variables are used to describe the dynamic response of the system based on Newton-Euler's laws.

[0037] First, establish the yaw moment balance equation (Formula 2): using the moment of inertia Describes the rotational dynamics of a vehicle about its vertical axis, using data including the front and rear wheelbases. and tire lateral stiffness The torque term quantifies the front wheel steering angle. The input drives changes in vehicle body posture.

[0038] Secondly, establish the lateral force balance equation (Formula 3): considering the overall vehicle mass longitudinal velocity relative to the vehicle's center of gravity Calculate the lateral acceleration of the center of mass caused by the lateral force of the tire, and introduce the eccentric term caused by the rotation of the coordinate system. The sideslip angle of the center of mass is obtained. The changes over time serve as the core physical model for estimating the system's sideslip angle.

[0039] (2) (3) In the formula, Indicates yaw acceleration; This indicates the rate of change of the vehicle's center of gravity sideslip angle; Indicates the vehicle's steering angle.

[0040] The initial state estimation module is used to obtain the initial state of the tire sideslip angle based on the extended kinematics model and the extended Kalman filter algorithm.

[0041] Rewrite equation (1) as state equation (4): (4) In the formula, This indicates the selection of a state variable; This indicates a control input.

[0042] Given the highly nonlinear nature of the aforementioned state equation (Equation 4), local linearization is required to adapt it for the iterative solution of the extended Kalman filter algorithm. The state vector is chosen as... The control input is For nonlinear functions Regarding the state vector Find the partial derivatives and calculate the time interval. State Jacobian matrix That is, formula (5); where, This represents the observed value of the lateral deviation; Indicates the observed heading deviation; superscript T Indicates transpose; This represents the estimated sideslip angles of the front and rear wheels.

[0043] (5) Lateral deviation obtained from preprocessed GNSS data in the vehicle motion state vector acquisition module and heading deviation The data constructs an observation vector, using the vehicle nonlinear state equation formula (4) as the system model, and locally linearizes the system model based on the state Jacobian matrix shown in formula (5), establishing the state transition model of the extended Kalman filter algorithm. Subsequently, according to the prediction and update steps of the extended Kalman filter, the state vector is recursively estimated: in the prediction stage, the current state and its covariance are predicted based on the linearized state transition model; in the update stage, the observation residuals are calculated using GNSS observations, and the predicted state is corrected to minimize the mean square error of the observation residuals. Through the above recursive filtering process, the real-time correction and convergence estimation of the state vector are realized, and finally, the estimated values ​​of the front wheel sideslip angle and the rear wheel sideslip angle are obtained from the converged state vector. .

[0044] The correction module is used to identify and correct the initial state of the tire sideslip angle online based on the two-degree-of-freedom dynamic model and the Lumberjack state observer, so as to obtain the corrected estimated values ​​of the front and rear tire sideslip angles.

[0045] like Figure 2 As shown, firstly, the estimated front wheel sideslip angle and the estimated rear wheel sideslip angle obtained from the initial state estimation module are... For input, based on the vehicle's front and rear wheelbase. Based on the geometric proportional relationship, the initial geometric observation value of the centroid sideslip angle is calculated using formula (6). .

[0046] (6) Select the initial geometric observation value of the centroid sideslip angle and yaw rate Constructing the system state vector Previously, wheel angle As input, the yaw rate measured by the sensor. This is the output.

[0047] Based on the two-degree-of-freedom dynamic model constructed by the model building module, the standard state space update equation is obtained, namely formula (7), and rewritten in the standard state space form. In order to meet the standardization requirements of matrix operation, the aforementioned dynamic variables are vectorized, as shown in formula (8).

[0048] (7) (8) In the formula, x Represents the system state vector; The derivative of the state vector; , These represent the rate of change of the center of mass sideslip angle and the yaw acceleration, respectively. , These are the vectorized representations of the sideslip angle and yaw rate, respectively. A Represents the state matrix; B Represents the input matrix; C This represents the output matrix.

[0049] Wherein, the state matrix A Input matrix B and output matrix C They are respectively: , , (9) Secondly, the constructed Luneburger observer structure is shown in equation (10): (10) In the formula, The derivative of the system state estimation vector; This represents the estimated value of the system state vector; H This represents the optimal state feedback gain matrix.

[0050] Next, in order to ensure the observation error It can converge to zero quickly and stably, where, x The error dynamic equation is given by formula (11), which represents the true state vector of the system.

[0051] (11) in: (12) In the formula, , These represent the actual values ​​of tire lateral stiffness for the front and rear wheels, respectively. , These represent the feedback gain for correcting the sideslip angle estimate and the feedback gain for correcting the yaw rate estimate, respectively.

[0052] The optimal state feedback gain matrix is ​​solved using the linear quadratic regulator (LQR) method. Observer performance index function Formula (13) is used.

[0053] (13) In the formula, Q Represents the state error weight matrix; R This represents the control input weight matrix.

[0054] Finally, the unique symmetric positive definite solution matrix is ​​obtained by solving the algebraic Riccati equation formula (14). And perform real-time status correction.

[0055] (14) Finally, the optimal state feedback gain matrix at the current moment is calculated using formula (15). .

[0056] (15) Substituting the optimal state feedback gain matrix into the observer equation, and utilizing the deviation between the yaw rate measured by the sensors and the model prediction, a closed-loop correction is performed on the state vector, ultimately yielding the corrected estimates of the front and rear wheel sideslip angles. .

[0057] The fusion weighting module is used to obtain a fusion weighting factor for the front and rear wheel sideslip angles based on the corrected front and rear tire sideslip angle estimates, vehicle speed, and vehicle steering angle, and to obtain the final front and rear wheel sideslip angle estimates based on the fusion weighting factor.

[0058] like Figure 3 As shown, in this embodiment, the weighting factor considering speed is as shown in formula (16), and the weighting factor considering steering angle is as shown in formula (17).

[0059] (16) (17) In the formula, k v Indicates the speed influence factor; This indicates the influence factor of the angle.

[0060] Considering that the front wheel sideslip angle is more sensitive to the steering angle, the fusion weighting factor for the front wheel sideslip angle is as follows: (18) In the formula, Indicates the front wheel speed influence factor; This represents the factor influencing the front wheel steering angle.

[0061] The rear wheel sideslip angle is not directly affected by steering input but is more sensitive to vehicle speed. Its fusion weighting factor is as follows: (19) In the formula, This indicates the rear wheel speed influence factor.

[0062] The estimated front and rear wheel sideslip angles obtained from the initial state estimation module are respectively... The estimated front and rear wheel sideslip angles obtained by the correction module Substituting these values ​​into the final fusion formula (20) yields the final estimated front and rear wheel sideslip angles. .

[0063] (20) Example 2: This embodiment provides a real-time estimation method for tractor sideslip angle based on assisted navigation. Based on the aforementioned estimation system, it includes the following steps: S1. Real-time acquisition of multi-source motion state information of the tractor using the tractor assisted navigation system, and processing of the multi-source motion state information to obtain a vehicle motion state vector; the assisted navigation system includes: a dual-antenna GNSS positioning unit, an IMU, a main controller, a host computer, a power management module, and a software module; S2. Construction of an extended kinematic model and a two-degree-of-freedom dynamic model based on the vehicle motion state vector; S3. Obtaining the initial state of the tire sideslip angle based on the extended kinematic model and the extended Kalman filter algorithm; S4. Online identification and correction of the initial state of the tire sideslip angle based on the two-degree-of-freedom dynamic model and the Luneburg state observer to obtain corrected estimates of the front and rear tire sideslip angles; S5. Obtaining a fusion weighting factor for the front and rear tire sideslip angles based on the corrected estimates, vehicle speed, and vehicle steering angle, and obtaining the final estimates of the front and rear tire sideslip angles based on the fusion weighting factor.

[0064] Example 3: This embodiment uses the Dongfanghong LY1104 wheeled tractor as a test platform to test and verify the real-time sideslip angle estimation method proposed in this invention.

[0065] To enhance the representation of vehicle sideslip during field operations, a relatively heavy implement, namely a Hongzhu 180D potato harvester, was connected to the tractor's rear three-point suspension system. Because the implement experiences significant soil resistance during operation, it is prone to causing noticeable lateral deviation of the tractor under straight-line navigation conditions. This provides a typical working condition for verifying the sideslip angle estimation performance of the algorithm of this invention in complex farmland environments.

[0066] The test site was selected at the Jiaozhou City Test Base in Qingdao. The test plot consisted of typical clay loam soil with moderate surface flatness, providing representative field operation conditions. During the test, the tractor traveled along a pre-planned straight working trajectory, and the auxiliary navigation system output the vehicle's heading angle, position information, speed information, and actuator control commands in real time. The true values ​​of the front wheel steering angle and speed used in this embodiment are as follows: Figure 4 As shown, the real-time sideslip angle estimation algorithm of the present invention is deployed in the main controller and runs online at an update frequency of 50 Hz. By fusing the heading, position and velocity information provided by GNSS, the attitude and steering angle information output by IMU, and the vehicle kinematics model and dynamics model, the real-time estimation of the sideslip angle is achieved.

[0067] In the algorithm initialization stage, based on the overall structural parameters of the tractor used, the relevant parameters in the constructed tractor kinematic model and dynamic model are set and substituted into formulas (1) to (3). The relevant parameters include: the wheelbase between the centers of the front and rear wheels, the distance from the vehicle's center of mass to the centers of the front and rear wheels, and the installation parameters of the GNSS antenna.

[0068] Subsequently, using the extended kinematic model formula (4) in S3 and combined with the extended Kalman filter algorithm, the front wheel sideslip angle and the rear wheel sideslip angle are initially estimated to obtain the initial estimated values ​​of the front and rear wheel sideslip angles. ; Further, the preliminary estimated value Substituting into formula (6), we obtain the initial value required for sideslip angle estimation based on the dynamic model in the correction module.

[0069] In the process of estimating the sideslip angle using the dynamic model in the correction module, the vehicle state variable is selected as... and input in a given system With output Under the condition of [condition], the state update equation is established according to formula (2) and formula (3), and the state equation is obtained as shown in formula (7). By linearizing the state equation near the system equilibrium point, the partial derivatives of each state variable are calculated, and the Luneburg state observer model is constructed as shown in formula (8). Furthermore, the dynamic equation of the state estimation error is established as shown in formula (11), where the system matrix and the output matrix are given by formula (12).

[0070] Based on the aforementioned error dynamic model, the observer gain is optimized using the linear quadratic regulator (LQR) method, and the observer performance index function is defined as shown in formula (13). By establishing a weighted optimization relationship between the state estimation error and the observer gain, a comprehensive constraint on the dynamic performance of the estimation error can be achieved.

[0071] Based on the above, by solving the algebraic Riccati equation as shown in formula (14), a unique symmetric positive definite solution matrix is ​​obtained, and the observer state is corrected in real time accordingly. Further, the optimal state feedback gain matrix at the current moment is calculated using formula (15), and this optimal state feedback gain matrix is ​​substituted into the observer equation to form a closed-loop state estimation structure. By introducing the deviation between the yaw rate measured by the sensor and the model prediction, the state vector is corrected online, and the corresponding front wheel sideslip angle and rear wheel sideslip angle estimates are calculated. .

[0072] Based on this, the vehicle speeds calculated via GNSS will be... Substituting the speed weighting factor formula (16), and the front wheel steering angle measured by the IMU... Substituting into the weighting factor formula (17) that considers the influence of steering angle, the speed weighting factor and steering angle weighting factor are calculated. Then, the weighting factors are substituted into the fusion weighting factor formulas (18) and (19) for the front wheel sideslip angle and the rear wheel sideslip angle, respectively, to obtain the fusion weighting coefficients for the front and rear wheel sideslip angles, as shown below. Figure 5 As shown. Finally, the sideslip angle estimate obtained from the extended Kalman filter is... The estimated sideslip angle obtained based on the dynamic model Substituting the corresponding fusion weight coefficients into formula (20), we obtain the following: Figure 6 The final estimates of the front and rear wheel sideslip angles are shown below. and .

[0073] The embodiments described above are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Various modifications and improvements made to the technical solutions of the present invention by those skilled in the art without departing from the spirit of the present invention should fall within the protection scope defined by the claims of the present invention.

Claims

1. A real-time estimation system for the sideslip angle of a tractor based on assisted navigation, characterized in that, include: The vehicle motion state vector acquisition module is used to acquire multi-source motion state information of the tractor in real time based on the tractor auxiliary navigation system, and process the multi-source motion state information to obtain the vehicle motion state vector; the auxiliary navigation system includes: a dual-antenna GNSS positioning unit, an IMU, a main controller, a host computer, a power management module, and a software module; The model building module is used to construct an extended kinematic model and a two-degree-of-freedom dynamic model based on the vehicle's motion state vector. An initial state estimation module is used to obtain the initial state of the tire sideslip angle based on the extended kinematics model and the extended Kalman filter algorithm. The correction module is used to identify and correct the initial state of the tire sideslip angle online based on the two-degree-of-freedom dynamic model and the Lumberjack state observer, so as to obtain the corrected estimated values ​​of the front and rear tire sideslip angles. The fusion weighting module is used to obtain a fusion weighting factor for the front and rear wheel sideslip angles based on the corrected front and rear tire sideslip angle estimates, vehicle speed, and vehicle steering angle, and to obtain the final front and rear wheel sideslip angle estimates based on the fusion weighting factor.

2. The real-time estimation system for tractor sideslip angle based on assisted navigation according to claim 1, characterized in that, The process by which the vehicle motion state vector acquisition module obtains the vehicle motion state vector includes: The dual-antenna GNSS positioning unit is used to acquire the real-time global position coordinates and heading information of the tractor to establish a navigation reference; the front wheel steering angle is obtained using the front wheel IMU. Use vehicle body IMU to monitor vehicle body yaw rate The system captures the vehicle's dynamic posture; it then performs digital filtering and noise reduction, timestamp synchronization and alignment, and extrinsic parameter calibration based on the installation location on the obtained raw data to obtain a spatiotemporally unified vehicle motion state vector.

3. The real-time estimation system for tractor sideslip angle based on assisted navigation according to claim 1, characterized in that, The extended kinematic model includes: ； In the formula, This represents the lateral velocity of the vehicle relative to the reference path. Indicates the longitudinal speed of the tractor; Indicates heading deviation; This represents the estimated final front wheel sideslip angle; This represents the estimated final rear wheel sideslip angle; This indicates the wheelbase of the vehicle.

4. The real-time estimation system for tractor sideslip angle based on assisted navigation according to claim 1, characterized in that, The two-degree-of-freedom dynamic model includes: ； ； In the formula, Indicates yaw acceleration; a , b These represent the front wheelbase and the rear wheelbase, respectively. Indicates the sideslip angle of the center of mass; Indicates the overall vehicle weight; Indicates the longitudinal velocity of the vehicle's center of gravity; Indicates the front wheel steering angle; These represent the lateral stiffness of the front and rear tires, respectively. It represents the moment of inertia.

5. A real-time estimation system for tractor sideslip angle based on assisted navigation according to claim 1, characterized in that, The process by which the initial state estimation module obtains the initial state of the tire sideslip angle includes: The extended kinematic model is expressed as a state equation, and the state vector is chosen as... The control input is For nonlinear functions Regarding the state vector Find the partial derivatives and calculate the time interval. State Jacobian matrix ;in, This represents the observed value of the lateral deviation; Indicates the observed heading deviation; superscript T Indicates transpose; This represents the estimated sideslip angles of the front and rear wheels; Based on the lateral deviation and heading deviation, an observation vector is constructed. Using the state equation as the system model, and based on the state Jacobian matrix, a state transition model of the extended Kalman filter algorithm is established. Based on the prediction and update steps of the extended Kalman filter algorithm, the state vector is recursively estimated, and the estimated values ​​of the front wheel sideslip angle and the rear wheel sideslip angle are obtained from the converged state vector.

6. A real-time tractor sideslip angle estimation system based on assisted navigation according to claim 1, characterized in that, The process by which the correction module obtains the corrected estimates of the front and rear tire sideslip angles includes: First, the estimated front wheel sideslip angle and the estimated rear wheel sideslip angle... For input, based on the vehicle's front and rear wheelbase. Based on the geometric proportional relationship, the initial geometric observation value of the centroid sideslip angle is calculated; Based on the aforementioned two-degree-of-freedom dynamic model, the standard state-space update equation is obtained; The Luneburger observer structure includes: ； In the formula, The derivative of the system state estimation vector; This represents the estimated value of the system state vector; A Represents the state matrix; B Represents the input matrix; C Indicates the output matrix; H This represents the optimal state feedback gain matrix; Construct the error dynamic equation and solve for the optimal state feedback gain matrix using the linear quadratic regulator method. ; Substituting the optimal state feedback gain matrix into the Romberg observer, and utilizing the deviation between the yaw rate measured by the sensors and the model prediction, a closed-loop correction is performed on the state vector to obtain the corrected estimates of the front and rear wheel sideslip angles. .

7. A real-time tractor sideslip angle estimation system based on assisted navigation according to claim 1, characterized in that, The fusion weighting factor for the front wheel sideslip angle includes: ； In the formula, Indicates the front wheel speed influence factor; Indicates the front wheel steering angle influence factor; The fusion weighting factor for the rear wheel sideslip angle includes: ； In the formula, This indicates the rear wheel speed influence factor.

8. A real-time tractor sideslip angle estimation system based on assisted navigation according to claim 7, characterized in that, The final estimates of the front and rear wheel sideslip angles include:

9. A real-time estimation method for the sideslip angle of a tractor based on assisted navigation, wherein the estimation method is applied to the estimation system according to any one of claims 1-8, characterized in that, Includes the following steps: S1. Based on the tractor auxiliary navigation system, multi-source motion state information of the tractor is acquired in real time, and the multi-source motion state information is processed to obtain the vehicle motion state vector; the auxiliary navigation system includes: a dual-antenna GNSS positioning unit, an IMU, a main controller, a host computer, a power management module, and a software module; S2. Construct an extended kinematic model and a two-degree-of-freedom dynamic model based on the vehicle motion state vector; S3. The initial state of the tire sideslip angle is obtained based on the extended kinematics model and the extended Kalman filter algorithm. S4. Based on the two-degree-of-freedom dynamic model and the Lumberjack state observer, the initial state of the tire sideslip angle is identified and corrected online to obtain the corrected estimated values ​​of the front and rear tire sideslip angles. S5. Based on the corrected estimated front and rear tire sideslip angles, vehicle speed, and vehicle steering angle, a fusion weighting factor for the front and rear tire sideslip angles is obtained, and the final estimated front and rear tire sideslip angles are obtained based on the fusion weighting factor.

Images