Agricultural machine path tracking control method and device, electronic equipment and storage medium

Abstract

The application discloses a kind of agricultural machinery path tracking control method, device, electronic equipment and storage medium, method includes: agricultural machinery is along target trajectory curve operation, according to fixed control cycle, the path state information of vehicle-mounted positioning fusion link output is collected;According to adjacent control time path state information, separate to obtain the lateral drift residual that cannot be explained by vehicle body course motion, the lateral disturbance caused by the residual that represents lateral slippage, steering lag, ground side slip;Front feed compensation amount is constructed based on lateral drift residual;To lateral error, course error is carried out time delay compensation prediction, and the initial lateral deviation of foresight point is solved;Final lateral deviation is obtained by using front feed compensation amount to correct initial lateral deviation, and target steering angle is solved;Target steering angle is issued to agricultural machinery steering mechanism to complete trajectory tracking.This scheme can compensate various lateral disturbances and system time delay, improve agricultural machinery path tracking precision.

Description

Technical Field

[0001] This invention relates to automatic control technology, to lateral control of automatic driving of mobile machinery, and particularly to a method, device, electronic device and storage medium for agricultural machinery path tracking control. Background Technology

[0002] In the operation of automated agricultural machinery, pure tracking algorithms are often used to achieve path tracking control of the target trajectory curve. The geometric model of the pure tracking algorithm implicitly assumes a key assumption: the lateral motion of the vehicle is entirely determined by the heading angle. This assumption means that the vehicle does not experience any sideslip, and the orientation of the wheels (or tracks) is the direction of motion.

[0003] However, in actual agricultural machinery autonomous driving scenarios, this assumption deviates significantly from real-world physical conditions. Agricultural machinery typically operates on unstructured surfaces such as muddy paddy fields and soft soil, resulting in significant lateral slippage between the tires (or tracks) and the ground. Furthermore, agricultural machinery generally employs hydraulic steering systems, and the dead zone, hysteresis, and flow nonlinearity of hydraulic valves lead to a non-negligible dynamic delay between the actual steering response and the command. If a pure tracking algorithm is still used to achieve path tracking control of the target trajectory curve, the trajectory tracking deviation will continuously increase, making it difficult to meet the requirements of high-precision agricultural machinery operations.

[0004] Therefore, a new method for tracking and controlling agricultural machinery is urgently needed. Summary of the Invention

[0005] This invention provides a method, device, electronic device, storage medium, and computer program product for agricultural machinery path tracking control.

[0006] According to one aspect of the present invention, a method for tracking and controlling agricultural machinery path is provided, comprising:

[0007] During the operation of the agricultural machinery along the preset target trajectory curve, the path status information synchronously output by the same vehicle positioning fusion link is continuously collected according to a fixed control cycle; the path status information includes at least lateral error, heading error and longitudinal speed of the agricultural machinery;

[0008] Based on the path state information of the current control time and the previous control time, the lateral drift residual that cannot be explained by the vehicle heading motion is separated; the lateral drift residual uniformly represents the lateral disturbance caused by vehicle lateral slip, steering execution dynamic lag and ground lateral slip.

[0009] Based on the lateral drift residual, the cumulative trajectory offset caused by drift is predicted and a feedforward compensation amount is constructed;

[0010] The lateral and heading errors contained in the path status information at the current control time are subjected to time delay compensation prediction processing. Based on the predicted lateral and heading errors after time delay compensation prediction, the initial lateral deviation of the foresight point is solved according to the pure tracking algorithm rules.

[0011] The initial lateral deviation is corrected based on the feedforward compensation amount to obtain the final lateral deviation of the forward view point, and the target steering angle is solved based on the final lateral deviation of the forward view point.

[0012] The target steering angle is transmitted to the agricultural machinery steering mechanism so that the agricultural machinery travels along the target trajectory curve.

[0013] According to another aspect of the present invention, an agricultural machinery path tracking control device is provided, comprising:

[0014] The data acquisition module is used to continuously acquire path status information synchronously output by the same vehicle positioning fusion link according to a fixed control cycle during the operation of agricultural machinery along a preset target trajectory curve; the path status information includes at least lateral error, heading error and longitudinal speed of agricultural machinery;

[0015] The lateral drift residual determination module is used to separate the lateral drift residual that cannot be explained by the vehicle heading motion based on the path state information of the current control time and the previous control time. The lateral drift residual uniformly represents the lateral disturbance caused by vehicle lateral slip, steering execution dynamic lag and ground lateral slip.

[0016] The feedforward compensation calculation module is used to predict the cumulative trajectory offset caused by the drift based on the lateral drift residual and construct the feedforward compensation amount;

[0017] The delay compensation process is used to perform delay compensation prediction processing on the lateral error and heading error contained in the path state information at the current control time. Based on the predicted lateral error and predicted heading error after delay compensation prediction, the initial lateral deviation of the foreground point is solved according to the pure tracking algorithm rules.

[0018] The feedforward correction module is used to correct the initial lateral deviation according to the feedforward compensation amount, obtain the final lateral deviation of the forward view point, and solve the target steering angle based on the final lateral deviation of the forward view point.

[0019] The control module is used to transmit the target steering angle to the agricultural machinery steering mechanism so that the agricultural machinery travels along the target trajectory curve.

[0020] According to another aspect of the present invention, an electronic device is provided, the electronic device comprising:

[0021] At least one processor; and

[0022] A memory that is communicatively connected to at least one processor; wherein,

[0023] The memory stores a computer program that can be executed by at least one processor, such that the at least one processor is able to execute the agricultural machinery path tracking control method of the present invention.

[0024] According to another aspect of the present invention, a computer-readable storage medium is provided, which stores computer instructions for causing a processor to execute and implement the agricultural machinery path tracking control method of the present invention.

[0025] According to another aspect of the present invention, a computer program product is provided, comprising a computer program that, when executed by a processor, implements the steps in the above-described method.

[0026] The technical solution of this invention can simultaneously cope with multiple disturbances such as control system delay, ground lateral slip, tire lateral slip, and dynamic lag in hydraulic steering, significantly improving the path tracking accuracy of agricultural machinery automatic driving under complex farmland conditions, effectively reducing steady-state trajectory deviation, and adapting to various agricultural machinery operation scenarios such as sowing and harvesting.

[0027] It should be understood that the description in this section is not intended to identify key or essential features of the embodiments of the present invention, nor is it intended to limit the scope of the invention. Other features of the invention will become readily apparent from the following description. Attached Figure Description

[0028] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying 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.

[0029] Figure 1 This is a flowchart illustrating the agricultural machinery path tracking control method provided in an embodiment of the present invention;

[0030] Figure 2 This is a flowchart illustrating another agricultural machinery path tracking control method provided in an embodiment of the present invention;

[0031] Figure 3 This is a schematic diagram of the structure of the agricultural machinery path tracking control device provided in an embodiment of the present invention;

[0032] Figure 4 This is a schematic diagram of the structure of an electronic device that implements the agricultural machinery path tracking control method of this invention. Detailed Implementation

[0033] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. 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 should fall within the scope of protection of the present invention.

[0034] Example 1

[0035] Figure 1 This is a flowchart of an agricultural machinery path tracking control method provided in an embodiment of the present invention. This embodiment is applicable to path tracking control scenarios in which agricultural machinery performs automatic farmland operation along a preset trajectory and suppresses lateral slippage and steering lag disturbances. The method can be executed by an agricultural machinery path tracking control device, which can be implemented in hardware and / or software and can be configured in an electronic device.

[0036] like Figure 1 As shown, the agricultural machinery path tracking control method includes steps S101-S106:

[0037] S101. During the operation of the agricultural machinery along the preset target trajectory curve, the path status information synchronously output by the same vehicle positioning fusion link is continuously collected according to a fixed control cycle; the path status information includes at least lateral error, heading error and longitudinal speed of the agricultural machinery.

[0038] S102. Based on the path state information of the current control time and the previous control time, the lateral drift residual that cannot be explained by the vehicle heading motion is separated; the lateral drift residual uniformly represents the lateral disturbance caused by vehicle lateral slip, steering execution dynamic lag and ground lateral slip.

[0039] S103. Based on the lateral drift residual, predict the cumulative trajectory offset caused by the drift and construct the feedforward compensation amount;

[0040] S104. Perform time delay compensation prediction processing on the lateral error and heading error contained in the path status information at the current control time. Based on the predicted lateral error and predicted heading error after time delay compensation prediction, solve the initial lateral deviation of the foresight point according to the pure tracking algorithm rules.

[0041] S105. Correct the initial lateral deviation according to the feedforward compensation amount to obtain the final lateral deviation of the forward view point, and solve the target steering angle based on the final lateral deviation of the forward view point.

[0042] S106. The target steering angle is transmitted to the agricultural machinery steering mechanism so that the agricultural machinery travels along the target trajectory curve.

[0043] In step S101, the target trajectory curve is a reference driving path pre-planned based on the farmland operation area and operation width before operation, providing a trajectory reference for the automatic driving operation of agricultural machinery; the fixed control cycle is the time interval between the on-board automatic driving controller performing a complete path tracking calculation and generating steering commands, and the controller runs the entire set of control logic in a cycle according to this cycle; the on-board positioning fusion link is a combined navigation system composed of a GNSS receiver and an inertial measurement unit (IMU), which outputs high-precision position and attitude information through a fusion filtering algorithm; the same on-board positioning fusion link represents that all state information comes from the same source calculation link, ensuring the consistency of data time reference; the path state information is the deviation information calculated by the positioning system after projecting the real-time position of the agricultural machinery onto the target trajectory curve, which may include lateral error, heading error, longitudinal speed of the agricultural machinery, and yaw rate, etc.; the lateral error is the lateral vertical distance (and shortest distance) between the current center of gravity position (rear axle center) of the agricultural machinery and the target trajectory curve; the heading error is the angle between the heading angle of the agricultural machinery body and the tangent direction of the current point of the target trajectory; the longitudinal speed of the agricultural machinery is the linear speed of the agricultural machinery along the forward direction of the vehicle body.

[0044] In some embodiments, step S101 is specifically implemented as follows: the agricultural machinery enters an autonomous driving operation mode and follows a preset target trajectory curve to carry out agricultural operations such as sowing and harvesting; the on-board controller triggers a data acquisition process according to a preset fixed control cycle; within a single control cycle, the controller synchronously reads a set of calculation results from the same set of on-board positioning fusion links to avoid timing misalignment caused by asynchronous acquisition of multiple sensors; the lateral error, heading error, and longitudinal speed of the agricultural machinery are extracted from the positioning output results as the raw input data for the current cycle path tracking algorithm;

[0045] The beneficial effects of this step are: using the same-source positioning fusion link to synchronously collect all state quantities, eliminating the calculation deviation caused by the time asynchrony of data from different sensors; periodically and continuously collecting data to construct a time-continuous observation sequence, providing a time-consistent, stable and reliable data foundation for subsequent lateral drift residual differential calculation and time delay recursive prediction.

[0046] In step S102, the current control time is the moment when the controller is executing the algorithm calculation and collecting data in this round; the previous control time is the moment when the calculation was executed in the previous control cycle; the duration between the current control time and the previous control time is equal to the fixed control cycle. Lateral drift residual: the disturbance component remaining after deducting the explainable component of the heading motion from the total lateral error change; this residual uniformly represents the lateral disturbances coupled from multiple sources such as farmland surface lateral slip, tire lateral slip, and hydraulic steering mechanism response lag, achieving integrated disturbance quantification.

[0047] In some embodiments, step S102 is specifically implemented by the following steps S1021-S1024:

[0048] S1021. By performing differential calculation on the lateral error at the current control moment and the lateral error at the previous control moment, the rate of change of the lateral error is obtained.

[0049] Among them, differential operation: in the discrete time domain, the rate of change of the signal is approximated by the sampled values ​​of adjacent control cycles; the rate of change of lateral error: the amount of change of lateral error per unit time, which represents the overall lateral change speed of lateral deviation, including the combined effect of normal steering motion of agricultural machinery and external drift disturbance.

[0050] In practice, the lateral error of reading the cache at the previous control time step (k-1) is considered. Read the lateral error of the current control time (k) in this round of data acquisition. Perform differential calculations according to a fixed control cycle: In the formula: This represents the rate of change of the lateral error. The fixed control cycle depends on the system communication frequency. In the first frame of system operation, since there is no historical data, the value is set to zero; the calculated value... The output is used as the input for the S1023 operation.

[0051] The rate of change of lateral error is obtained by differentiating the periodic sampling data, which quantifies the overall lateral movement trend of agricultural machinery. The S101 fixed period ensures the consistency of the differential time reference, avoids the distortion of the rate of change calculation caused by periodic fluctuations, and provides an observation basis for subsequent disturbance decoupling.

[0052] S1022. Based on the heading error and longitudinal speed of the agricultural machinery at the current control moment, and in conjunction with the agricultural machinery motion model, calculate the lateral velocity component that can be explained by the heading motion.

[0053] Among them, the lateral velocity component that can be explained by heading motion is the theoretical lateral velocity that can be generated solely by the agricultural machinery's own turning and heading change. This component belongs to normal motion and does not belong to external disturbance drift.

[0054] In practice, the heading error at the current control moment is obtained. Longitudinal speed of agricultural machinery Based on the following agricultural machinery planar motion model, calculate the lateral velocity component that can be explained by the heading motion: In the formula: The lateral velocity component is interpretable for heading motion. Output The data is fed into S1023 to participate in the residual separation calculation.

[0055] The beneficial effects of this step are: establishing a theoretical model of the normal heading motion of agricultural machinery, quantifying the lateral motion caused by active steering; decoupling the normal motion from the external disturbance model, providing a theoretical benchmark for eliminating the normal steering component and extracting drift disturbances, and preventing the active steering behavior of agricultural machinery from being misidentified as lateral drift.

[0056] S1023. Subtract the lateral velocity component from the lateral error change rate to obtain the original lateral drift residual.

[0057] The original lateral drift residual is the remaining term after deducting the normal heading motion component from the total lateral change rate; it includes various coupled disturbances such as lateral slip, ground yaw, and steering execution lag, and is superimposed with positioning random noise, which has not yet been filtered.

[0058] Specifically, the original lateral drift residual is calculated according to the following formula. :

[0059] The original lateral drift residual is passed to S1024 for filtering.

[0060] This step achieves disturbance separation through model subtraction, unifying various lateral disturbances with different mechanisms into a single residual signal; it eliminates the need to establish complex dynamic models for slip, steering hysteresis, and ground yaw separately, significantly reducing the computational load on the controller; the obtained original residual signal intuitively represents the sum of various unmodeled lateral disturbances.

[0061] S1024. The original lateral drift residual is smoothed and filtered to obtain the final lateral drift residual that cannot be explained by the vehicle heading motion.

[0062] Among them, smoothing filtering can be achieved by using IIR infinite impulse response low-pass filtering; the final lateral drift residual refers to the steady-state signal of the original residual after filtering out high-frequency noise, which is equivalent to the continuously acting lateral drift velocity.

[0063] Specifically, the following formula can be used for filtering: ;in, These are the filter coefficients. ; The IIR time constant can be selected based on the frequency characteristics of the sideslip signal and the level of differential noise, achieving a balance between suppressing differential noise and preserving the true sideslip signal. It is the original lateral drift residual; It is the final lateral drift residual at the current control moment; It is the final lateral drift residual from the previous control moment.

[0064] This step can filter out high-frequency random noise from the positioning system and suppress severe jitter in the residual signal; prevent noise from directly entering the feedforward channel and causing frequent oscillations in the steering command; output smooth and stable drift characteristics to ensure continuous and stable feedforward compensation and improve the smoothness of agricultural machinery operation.

[0065] The above S1021-S1024 can decouple and distinguish the normal heading motion of agricultural machinery from external lateral disturbances, avoiding misidentification of active steering motion as drift disturbance; various lateral disturbances with different mechanisms are uniformly collected into a single residual quantity, eliminating the need for separate modeling of various disturbances; IIR filtering suppresses observation noise, ensuring stable calculation of subsequent feedforward compensation quantities and preventing frequent jittering of steering commands.

[0066] In step S103, the cumulative trajectory offset caused by the drift is predicted based on the lateral drift residual, and a feedforward compensation amount is constructed, including:

[0067] The feedforward compensation amount is obtained by multiplying the smoothed filter-processed lateral drift residual, the preset prediction time domain, and the feedforward gain; the feedforward compensation amount represents the cumulative trajectory offset caused by the continuous effect of lateral drift in the prediction time domain.

[0068] Among them, the lateral drift residual after smoothing filtering is the final lateral drift residual output by S1024. Physically equivalent to the lateral drift velocity continuously acting on the agricultural machinery, this filter out high-frequency positioning noise and represents the true steady-state lateral disturbance; prediction time domain The pre-set forward estimation time, matched with the total delay of the entire control system (positioning transmission delay, controller calculation delay, hydraulic steering execution lag delay), represents the prediction time that needs to be compensated in advance; feedforward gain. Adjustable compensation coefficient, which can be obtained through field calibration, is used to adapt to different ground conditions and adjust the strength of feedforward compensation; Feedforward compensation amount: Characterizes the lateral trajectory offset formed by the cumulative effect of continuous lateral drift in the prediction time domain, and is used to compensate for drift disturbances in advance.

[0069] Specifically, the feedforward compensation amount can be calculated using the following formula: The calculated feedforward compensation is output and supplied to subsequent S105 to correct the initial lateral deviation of the forward viewpoint. It can be understood that drift velocity × time = drift-induced lateral offset; then, the compensation strength is adjusted by the gain coefficient to predict the cumulative drift deviation over a future period.

[0070] The beneficial effects of this step are as follows: Unlike traditional pure feedback control, which can only eliminate deviations with lag, this step predicts the cumulative offset of disturbances over a period of time based on drift speed and generates a forward-looking feedforward compensation term; it can suppress continuous lateral drift in advance, reduce steady-state trajectory deviation, and improve the ability of agricultural machinery to maintain its trajectory on wet and soft farmland.

[0071] In step S104, the time-delay compensation prediction processing is performed on the lateral error and heading error included in the path state information at the current control time. Based on the predicted lateral error and predicted heading error after time-delay compensation prediction, the initial lateral deviation of the foresight point is solved according to the pure tracking algorithm rules, including steps S1041-S1044:

[0072] S1041. Using the lateral error, heading error, and longitudinal speed of the agricultural machinery at the current control moment, set the prediction time domain, the number of recursion steps N, and the single-step recursion time step, and carry out N-step open-loop state recursion calculation.

[0073] Wherein, the lateral error at the current control moment is The heading error is And the longitudinal speed of the agricultural machinery is v; the prediction time domain is The time step for single-step recursion is , Open-loop state recursion: The deduction process no longer introduces new external observation feedback, but only relies on the motion model to iteratively simulate the future motion trend of the agricultural machinery to compensate for system delay.

[0074] S1042. Within each step of the recursion, the state is updated according to the agricultural machinery planar motion model: the lateral error is updated incrementally based on the agricultural machinery longitudinal speed, heading error and single-step recursion time step; the heading error is updated incrementally based on the heading change rate and single-step recursion time step.

[0075] In the specific implementation, the following update is performed on the i-th step of the recursion:

[0076] The formula for updating the lateral error increment is as follows: ;

[0077] Heading error increment update formula: ;in, The heading change rate is physically equivalent to the yaw rate of the agricultural machinery; in particular, the heading change rate gradually decreases during the recursive process, which indicates a decrease in the reliability of long-term heading prediction.

[0078] Use the updated state as the input for the next round of recursion, and repeat until all N steps are completed.

[0079] The advantages of single-step operation are: relying on kinematic models to simulate the natural motion evolution of agricultural machinery within a time delay window to achieve state prediction; introducing a heading change rate decay mechanism to suppress prediction divergence caused by long-term open-loop recursion; fully considering long-term motion uncertainties to prevent serious distortion of prediction results and ensure that the predicted end-time state has engineering effectiveness.

[0080] S1043. After completing all N iterative operations, the predicted lateral error and predicted heading error corresponding to the predicted time domain endpoint are obtained.

[0081] The prediction lateral error and prediction heading error corresponding to the prediction time domain endpoint are respectively , .

[0082] S1044. Calculate the adaptive forward sight distance based on the longitudinal speed of the agricultural machinery. Combine the predicted lateral error, predicted heading error and forward sight distance, and solve for the initial lateral deviation of the forward sight point according to the geometric relationship of the pure tracking algorithm.

[0083] Among them, the adaptive forward-looking distance satisfies ;in, To adapt to the forward sight distance, For minimum forward sight distance, Here, v is the speed gain coefficient, and v is the longitudinal speed of the agricultural machinery. The forward look-ahead point is a certain distance (forward look distance) in front of the agricultural machinery along the target trajectory. The reference point selected at () is used to determine the target location that the vehicle should drive to.

[0084] In some embodiments, the initial lateral deviation of the foresight point can be calculated using the following formula: .

[0085] The beneficial effects of this step are as follows: by compensating for the inherent time delay of the control system through open-loop multi-step recursive compensation, the tracking lag problem caused by time delay is solved; the heading change rate decay mechanism effectively suppresses the divergence of long-term motion prediction and prevents serious distortion of the prediction state; the vehicle speed adaptive forward sight distance enables the pure tracking algorithm to have good stability in both low-speed operation and high-speed transfer conditions, and obtains the basic forward sight point deviation after time delay compensation.

[0086] In step S105, the final lateral deviation is the deviation value after the initial lateral deviation of the forward view is superimposed with the drift feedforward compensation; the target steering angle is the expected front wheel steering angle command output by the upper-level path tracking algorithm and sent to the steering actuator.

[0087] In some embodiments, the initial lateral deviation is corrected according to the feedforward compensation amount to obtain the final lateral deviation of the foresee point, and the target steering angle is solved based on the final lateral deviation of the foresee point, including steps S1051-S1052:

[0088] S1051. The feedforward compensation amount is superimposed on the initial lateral deviation of the foreground point to obtain the final lateral deviation of the foreground point.

[0089] Specifically, the final lateral deviation of the foresight point is calculated using the following formula. :

[0090] ;

[0091] in, This represents the initial lateral deviation from the forward viewpoint; This is the feedforward compensation amount for construction.

[0092] S1052. Based on the final lateral deviation of the forward view point, the forward view distance, and the wheelbase of the agricultural machinery axle, the target steering angle is calculated using the pure tracking geometry formula.

[0093] Specifically, the target steering angle is calculated using the following formula:

[0094] ;

[0095] in, L is the target steering angle; L is the wheelbase of the agricultural machinery axle; Ld is the forward sight distance; It is the final lateral error of the forward viewpoint.

[0096] The benefits of this step are: it unifies and integrates the time delay compensation results and the drift disturbance feedforward compensation results at the pure tracking forward view lateral deviation level; it does not require separate modification of the underlying steering closed loop, achieving an organic combination of the two compensation mechanisms; the generated target steering angle takes into account both system time delay and continuous lateral drift disturbance, with stronger command foresight, reducing trajectory deviation from the source.

[0097] In step S106, transmitting the target steering angle to the agricultural machinery steering mechanism so that the agricultural machinery travels along the target trajectory curve includes: performing coordinate transformation and amplitude limiting processing on the solved target steering angle; sending the processed target steering angle to the agricultural machinery hydraulic steering actuator; collecting the actual steering angle fed back by the agricultural machinery steering wheel encoder or hydraulic cylinder displacement sensor, calculating the steering angle deviation between the target steering angle and the actual steering angle; using a closed-loop control algorithm to drive the hydraulic valve to work according to the steering angle deviation, controlling the hydraulic cylinder to act, adjusting the steering angle of the agricultural machinery's front wheels, and continuously guiding the agricultural machinery to travel along the target trajectory curve.

[0098] In this embodiment of the invention, firstly, the lateral drift residual, which uniformly represents various lateral disturbances, is decoupled and coupled using the path state information difference between adjacent control cycles. After filtering and smoothing, a drift feedforward compensation amount is constructed to compensate for continuous lateral disturbances in advance. Simultaneously, an N-step open-loop state recursion is used to perform time delay compensation prediction, and a heading change rate attenuation mechanism is introduced to avoid long-term prediction distortion. Furthermore, vehicle speed adaptive forward look distance is configured to optimize the pure tracking forward look point deviation solution. Subsequently, at the forward look point deviation level, the drift feedforward compensation and time delay compensation results are integrated to solve for the target steering angle. Finally, the target steering angle is sent to the hydraulic steering actuator, and precise steering execution is achieved through a closed-loop steering mechanism. This invention can simultaneously address multiple disturbances such as control system time delay, ground lateral slip, tire lateral slip, and hydraulic steering dynamic lag, significantly improving the path tracking accuracy of agricultural machinery automatic driving under complex farmland conditions, effectively reducing steady-state trajectory deviation, and adapting to various agricultural machinery operation scenarios such as sowing and harvesting.

[0099] Example 2

[0100] Figure 2 This is a flowchart illustrating a method for tracking and controlling agricultural machinery according to an embodiment of the invention. Figure 2 As shown, the process of the agricultural machinery path tracking control method includes the following steps:

[0101] S201. During the operation of the agricultural machinery along the preset target trajectory curve, the path status information synchronously output by the same vehicle positioning fusion link is continuously collected according to a fixed control cycle; the path status information includes at least lateral error, heading error and longitudinal speed of the agricultural machinery.

[0102] S202. Based on the path state information of the current control time and the previous control time, the lateral drift residual that cannot be explained by the vehicle heading motion is separated; the lateral drift residual uniformly represents the lateral disturbance caused by vehicle lateral slip, steering execution dynamic lag and ground lateral slip.

[0103] The specific implementation process of steps S201-S202 can be found in the above embodiments, and will not be repeated here.

[0104] S203. Based on the lateral drift residual, predict the cumulative trajectory offset caused by the drift and construct the feedforward compensation amount.

[0105] The calculated lateral drift residual is The constructed feedforward compensation amount is In this context, gain k is a lumped calibration parameter that integrates the prediction time domain and geometric transformation coefficients into the gain k, eliminating the need for explicit multiplication by the prediction time domain.

[0106] S204. Perform time delay compensation prediction processing on the lateral error and heading error contained in the path status information at the current control time. Based on the predicted lateral error and predicted heading error after time delay compensation prediction, solve the initial lateral deviation of the foresight point according to the pure tracking algorithm rules.

[0107] The specific implementation process of step S204 can be found in the description of the above embodiments, and will not be repeated here.

[0108] S205. Based on the initial lateral deviation of the forward viewpoint, and combined with the standard pure tracking circular arc geometry formula, calculate the initial target steering angle.

[0109] After time-delay compensation prediction processing, the predicted lateral error and predicted heading error have been obtained, and the initial lateral deviation of the forward look-out point has been solved. This initial lateral deviation only compensates for the inherent time delay of the control system and has not yet introduced the correction effect of lateral drift disturbance. This step retrieves fixed structural parameters such as adaptive forward look-out distance and machine wheelbase, substitutes the initial lateral deviation of the forward look-out point into the kinematic geometry relationship corresponding to pure tracking, and solves for the initial target steering angle. This initial target steering angle is the expected front wheel steering angle required to guide the machine to the target forward look-out point without considering continuous lateral drift disturbance, and serves as the basis for subsequent steering angle correction.

[0110] S206. Based on the feedforward compensation amount, the initial target steering angle is corrected to obtain the final target steering angle.

[0111] This implementation does not superimpose drift-related compensation amounts onto the forward look-ahead deviation position; instead, it performs corrections at the initial target steering angle output. For example, the feedforward compensation amount is directly superimposed onto the initial target steering angle to obtain the final target steering angle. The final target steering angle simultaneously considers both the system delay compensation effect and the need to suppress drift disturbances caused by lateral slip, ground yaw, and steering execution lag.

[0112] S207. The target steering angle is transmitted to the agricultural machinery steering mechanism so that the agricultural machinery travels along the target trajectory curve.

[0113] This scheme first decouples lateral drift residuals—characterized by lateral slip, ground yaw, and steering execution lag—from uniformly sourced positioning data with equal periods, constructing a drift feedforward correction term. Then, it performs time delay compensation prediction through open-loop state recursion, calculating the initial target steering angle based on a pure tracking algorithm. Subsequently, the drift feedforward correction is directly superimposed at the steering angle output to obtain the final target steering angle, which is then sent to the steering mechanism for closed-loop execution. This approach compensates for the inherent transmission and execution delays of the control system and proactively suppresses continuous lateral drift, reducing steady-state trajectory deviations in complex farmland conditions. Furthermore, drift compensation acts independently on the steering angle output, requiring no modification to the core pure tracking logic. The algorithm is highly modular, computationally simple, and easily adaptable to existing agricultural machinery autopilot controllers, facilitating parameter tuning.

[0114] Example 3

[0115] Figure 3 This is a schematic diagram of an agricultural machinery path tracking control device provided in an embodiment of the present invention. This device can execute any of the agricultural machinery path tracking control methods of the present invention. For example... Figure 3 As shown, the agricultural machinery path tracking control device includes:

[0116] The data acquisition module 301 is used to continuously acquire path status information synchronously output by the same vehicle positioning fusion link according to a fixed control cycle during the operation of agricultural machinery along a preset target trajectory curve; the path status information includes at least lateral error, heading error and longitudinal speed of agricultural machinery.

[0117] The lateral drift residual determination module 302 is used to separate the lateral drift residual that cannot be explained by the vehicle heading motion based on the path state information of the current control time and the previous control time; the lateral drift residual uniformly represents the lateral disturbance caused by vehicle lateral slip, steering execution dynamic lag and ground lateral slip.

[0118] Feedforward compensation calculation module 303 is used to predict the cumulative trajectory offset caused by the drift based on the lateral drift residual and construct the feedforward compensation amount;

[0119] The delay compensation processing module 304 is used to perform delay compensation prediction processing on the lateral error and heading error contained in the path status information at the current control time. Based on the predicted lateral error and predicted heading error after delay compensation prediction, the initial lateral deviation of the foresight point is solved according to the pure tracking algorithm rules.

[0120] The feedforward correction module 305 is used to correct the initial lateral deviation according to the feedforward compensation amount, obtain the final lateral deviation of the forward view point, and solve the target steering angle based on the final lateral deviation of the forward view point.

[0121] The control module 306 is used to transmit the target steering angle to the agricultural machinery steering mechanism so that the agricultural machinery travels along the target trajectory curve.

[0122] In some embodiments, the lateral drift residual determination module 302 is specifically used for:

[0123] The rate of change of the lateral error is obtained by performing a differential operation on the lateral error at the current control moment and the lateral error at the previous control moment.

[0124] Based on the heading error and longitudinal speed of the agricultural machinery at the current control moment, and combined with the agricultural machinery motion model, the lateral velocity component that can be explained by the heading motion is calculated.

[0125] The original lateral drift residual is obtained by subtracting the lateral velocity component from the lateral error change rate.

[0126] The original lateral drift residual is smoothed and filtered to obtain the final lateral drift residual that cannot be explained by the vehicle heading motion.

[0127] In some embodiments, the delay compensation processing module 304 is specifically used for:

[0128] Using the lateral error, heading error, and longitudinal speed of the agricultural machinery at the current control moment, the prediction time domain, the number of recursion steps N, and the single-step recursion time step are set, and N-step open-loop state recursion calculations are carried out.

[0129] Within each step of the recursion, the state is updated according to the agricultural machinery planar motion model: the lateral error is updated incrementally based on the agricultural machinery longitudinal speed, heading error and single-step recursion time step; the heading error is updated incrementally based on the heading change rate and single-step recursion time step; among them, the heading change rate gradually decreases during the recursion process, which indicates that the reliability of the long-term heading prediction is decreasing.

[0130] After completing all N iterative calculations, the predicted lateral error and predicted heading error corresponding to the predicted time domain endpoint are obtained;

[0131] The adaptive forward sight distance is calculated based on the longitudinal speed of the agricultural machinery. The initial lateral deviation of the forward sight point is obtained by combining the predicted lateral error, the predicted heading error and the forward sight distance, and solving the geometric relationship of the pure tracking algorithm.

[0132] In some embodiments, the adaptive forward look-ahead distance satisfies ;in, To adapt to the forward sight distance, For minimum forward sight distance, denoted as the speed gain coefficient, and v is the longitudinal speed of the agricultural machinery.

[0133] In some embodiments, the feedforward compensation calculation module 303 is specifically used for:

[0134] The feedforward compensation amount is obtained by multiplying the smoothed filter-processed lateral drift residual, the preset prediction time domain, and the feedforward gain; the feedforward compensation amount represents the cumulative trajectory offset caused by the continuous effect of lateral drift in the prediction time domain.

[0135] In some embodiments, the feedforward correction module 305 is specifically used for:

[0136] The feedforward compensation is added to the initial lateral deviation of the foreground point to obtain the final lateral deviation of the foreground point;

[0137] The target steering angle is calculated using pure tracking geometry formulas based on the final lateral deviation of the forward view, the forward view distance, and the wheelbase of the agricultural machinery axle.

[0138] In some embodiments, the control module 306 is specifically used for:

[0139] The obtained target steering angle is subjected to coordinate transformation and amplitude limiting.

[0140] The processed target steering angle is sent to the agricultural machinery hydraulic steering actuator;

[0141] Collect the actual steering angle fed back by the agricultural machinery steering wheel encoder or hydraulic cylinder displacement sensor, and calculate the steering angle deviation between the target steering angle and the actual steering angle;

[0142] A closed-loop control algorithm is used to drive the hydraulic valve based on the angle deviation, control the hydraulic cylinder, adjust the front wheel angle of the agricultural machinery, and continuously guide the agricultural machinery to travel along the target trajectory curve.

[0143] The agricultural machinery path tracking control device provided in the embodiments of the present invention can execute the agricultural machinery path tracking control method provided in any embodiment of the present invention, and has the corresponding functional modules and beneficial effects of the method.

[0144] According to embodiments of the present invention, the present invention also provides an electronic device, a readable storage medium, and a computer program product.

[0145] Example 4

[0146] Figure 4 A schematic diagram of the structure of an electronic device 10 that can be used to implement embodiments of the present invention is shown. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the invention described and / or claimed herein.

[0147] like Figure 4As shown, the electronic device 10 includes at least one processor 11 and a memory, such as a read-only memory 12 or a random access memory 13, communicatively connected to the at least one processor 11. The memory stores computer programs executable by the at least one processor. The processor 11 can perform various appropriate actions and processes based on the computer program stored in the read-only memory 12 or loaded from storage unit 18 into the random access memory 13. The random access memory 13 may also store various programs and data required for the operation of the electronic device 10. The processor 11, read-only memory 12, and random access memory 13 are interconnected via a bus 14. An input / output interface 15 is also connected to the bus 14.

[0148] Multiple components in electronic device 10 are connected to input / output interface 15, including: input unit 16; output unit 17, such as various types of displays, speakers, etc.; storage unit 18, such as disks, optical disks, etc.; and communication unit 19, such as network interface cards, modems, wireless transceivers, etc. Communication unit 19 allows electronic device 10 to exchange information / data with other devices through computer networks such as the Internet and / or various telecommunications networks.

[0149] Processor 11 can be a variety of general-purpose and / or special-purpose processing components with processing and computing capabilities. Some examples of processor 11 include, but are not limited to, central processing units, graphics processing units, various special-purpose artificial intelligence computing chips, various processors running machine learning model algorithms, digital signal processors, and any suitable processor, controller, microcontroller, etc. Processor 11 performs the various methods and processes described above, such as performing agricultural machinery path tracking control methods.

[0150] In some embodiments, the agricultural machinery path tracking control method may be implemented as a computer program tangibly contained in a computer-readable storage medium, such as storage unit 18. In some embodiments, part or all of the computer program may be loaded and / or installed on electronic device 10 via read-only memory 12 and / or communication unit 19. When the computer program is loaded into random access memory 13 and executed by processor 11, one or more steps of the agricultural machinery path tracking control method described above may be performed. Alternatively, in other embodiments, processor 11 may be configured to perform the agricultural machinery path tracking control method by any other suitable means (e.g., by means of firmware).

[0151] Various embodiments of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays, application-specific integrated circuits (ASICs), application-specific standard products (ASICs), systems-on-a-chip (SoCs), complex programmable logic devices, computer hardware, firmware, software, and / or combinations thereof. These various embodiments may include implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transmitting data and instructions to the storage system, the at least one input device, and the at least one output device.

[0152] Computer programs used to implement the methods of the present invention can be written in any combination of one or more programming languages. These computer programs can be provided to the processor of a general-purpose computer, a special-purpose computer, or other programmable agricultural machinery path tracking control device, such that when executed by the processor, the computer programs cause the functions / operations specified in the flowcharts and / or block diagrams to be implemented. The computer programs can be executed entirely on the machine, partially on the machine, as a standalone software package partially on the machine and partially on a remote machine, or entirely on a remote machine or server.

[0153] In the context of this invention, a computer-readable storage medium can be a tangible medium that may contain or store a computer program for use by or in conjunction with an instruction execution system, apparatus, or device. A computer-readable storage medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination thereof. Alternatively, a computer-readable storage medium may be a machine-readable signal medium. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory, read-only memory, erasable programmable read-only memory, optical fibers, portable compact disk read-only memory, optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0154] To provide interaction with a user, the systems and techniques described herein can be implemented on an electronic device having: a display device or liquid crystal display for displaying information to a user; and a keyboard and pointing device (e.g., a mouse or trackball) through which the user provides input to the electronic device. Other types of devices can also be used to provide interaction with a user; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including sound input, voice input, or tactile input).

[0155] The systems and technologies described herein can be implemented in computing systems that include backend components (e.g., as data servers), or middleware components (e.g., application servers), or frontend components (e.g., user computers with graphical user interfaces or web browsers through which users can interact with implementations of the systems and technologies described herein), or any combination of such backend, middleware, or frontend components. The components of the system can be interconnected via digital data communication of any form or medium (e.g., communication networks). Examples of communication networks include local area networks (LANs), wide area networks (WANs), blockchain networks, and the Internet. The computing system can include clients and servers. Clients and servers are generally geographically separated and typically interact via communication networks. Client-server relationships are created by computer programs running on the respective computers and having client-server relationships with each other. The server can be a cloud server, also known as a cloud computing server or cloud host, a host product within the cloud computing service system, addressing the shortcomings of traditional physical hosts and virtual private servers, such as high management difficulty and weak business scalability.

[0156] It should be understood that the various forms of processes shown above can be used, with steps reordered, added, or deleted. For example, the steps described in this invention can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution of this invention can be achieved, and this is not limited herein.

[0157] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A method for tracking and controlling the path of agricultural machinery, characterized in that, include: During the operation of agricultural machinery along the preset target trajectory curve, the path status information synchronously output by the same vehicle positioning fusion link is continuously collected according to a fixed control cycle; The path status information includes at least lateral error, heading error, and longitudinal speed of the agricultural machinery. Based on the path state information of the current control time and the previous control time, the lateral drift residual that cannot be explained by the vehicle heading motion is separated; the lateral drift residual uniformly represents the lateral disturbance caused by vehicle lateral slip, steering execution dynamic lag and ground lateral slip. Based on the lateral drift residual, the cumulative trajectory offset caused by drift is predicted and a feedforward compensation amount is constructed; The lateral and heading errors contained in the path status information at the current control time are subjected to time delay compensation prediction processing. Based on the predicted lateral and heading errors after time delay compensation prediction, the initial lateral deviation of the foresight point is solved according to the pure tracking algorithm rules. The initial lateral deviation is corrected based on the feedforward compensation amount to obtain the final lateral deviation of the forward view point, and the target steering angle is solved based on the final lateral deviation of the forward view point. The target steering angle is transmitted to the agricultural machinery steering mechanism so that the agricultural machinery travels along the target trajectory curve.

2. The method according to claim 1, characterized in that, The step of separating the lateral drift residuals that cannot be explained by the vehicle's heading motion based on the path state information of the current control time and the previous control time includes: The rate of change of the lateral error is obtained by performing a difference operation on the lateral error at the current control moment and the lateral error at the previous control moment. Based on the heading error and longitudinal speed of the agricultural machinery at the current control moment, and combined with the agricultural machinery motion model, the lateral velocity component that can be explained by the heading motion is calculated. The original lateral drift residual is obtained by subtracting the lateral velocity component from the lateral error change rate. The original lateral drift residual is smoothed and filtered to obtain the final lateral drift residual that cannot be explained by the vehicle heading motion.

3. The method according to claim 1, characterized in that, The process of performing time-delay compensation prediction processing on the lateral and heading errors included in the path state information at the current control time, and then solving for the initial lateral deviation of the foresight point according to the pure tracking algorithm rules based on the predicted lateral and heading errors after time-delay compensation prediction, includes: Using the lateral error, heading error, and longitudinal speed of the agricultural machinery at the current control moment, the prediction time domain, the number of recursion steps N, and the single-step recursion time step are set, and N-step open-loop state recursion calculations are carried out. Within each step of the recursion, the state is updated according to the agricultural machinery planar motion model: the lateral error is updated incrementally based on the agricultural machinery longitudinal speed, heading error and single-step recursion time step; the heading error is updated incrementally based on the heading change rate and single-step recursion time step; among them, the heading change rate gradually decreases during the recursion process, which indicates that the reliability of the long-term heading prediction is decreasing. After completing all N iterative calculations, the predicted lateral error and predicted heading error corresponding to the predicted time domain endpoint are obtained; The adaptive forward sight distance is calculated based on the longitudinal speed of the agricultural machinery. The initial lateral deviation of the forward sight point is obtained by combining the predicted lateral error, the predicted heading error and the forward sight distance, and solving the geometric relationship of the pure tracking algorithm.

4. The method according to claim 3, characterized in that, The adaptive forward look distance satisfies ;in, To adapt to the forward sight distance, For minimum forward sight distance, denoted as the speed gain coefficient, and v is the longitudinal speed of the agricultural machinery.

5. The method according to claim 2, characterized in that, Based on the lateral drift residual, the cumulative trajectory offset caused by the drift is predicted and a feedforward compensation amount is constructed, including: The feedforward compensation amount is obtained by multiplying the smoothed filter-processed lateral drift residual, the preset prediction time domain, and the feedforward gain; the feedforward compensation amount represents the cumulative trajectory offset caused by the continuous effect of lateral drift in the prediction time domain.

6. The method according to claim 3 or 5, characterized in that, The initial lateral deviation is corrected based on the feedforward compensation amount to obtain the final lateral deviation of the foreseeability point, and the target steering angle is solved based on the final lateral deviation of the foreseeability point, including: The feedforward compensation is added to the initial lateral deviation of the foreground point to obtain the final lateral deviation of the foreground point; Based on the final lateral deviation of the forward view, the forward view distance, and the wheelbase of the agricultural machinery axle, the target steering angle is calculated using pure tracking geometry formulas.

7. The method according to claim 1, characterized in that, The step of transmitting the target steering angle to the agricultural machinery steering mechanism so that the agricultural machinery travels along the target trajectory curve includes: The obtained target steering angle is subjected to coordinate transformation and amplitude limiting. The processed target steering angle is sent to the agricultural machinery hydraulic steering actuator; Collect the actual steering angle fed back by the agricultural machinery steering wheel encoder or hydraulic cylinder displacement sensor, and calculate the steering angle deviation between the target steering angle and the actual steering angle; A closed-loop control algorithm is used to drive the hydraulic valve based on the angle deviation, control the hydraulic cylinder, adjust the front wheel angle of the agricultural machinery, and continuously guide the agricultural machinery to travel along the target trajectory curve.

8. A path tracking and control device for agricultural machinery, characterized in that, include: The data acquisition module is used to continuously acquire path status information synchronously output by the same vehicle positioning fusion link according to a fixed control cycle during the operation of agricultural machinery along a preset target trajectory curve; the path status information includes at least lateral error, heading error and longitudinal speed of agricultural machinery; The lateral drift residual determination module is used to separate the lateral drift residual that cannot be explained by the vehicle heading motion based on the path state information of the current control time and the previous control time. The lateral drift residual uniformly represents the lateral disturbance caused by vehicle lateral slip, steering execution dynamic lag and ground lateral slip. The feedforward compensation calculation module is used to predict the cumulative trajectory offset caused by the drift based on the lateral drift residual and construct the feedforward compensation amount; The delay compensation processing module is used to perform delay compensation prediction processing on the lateral error and heading error contained in the path status information at the current control time. Based on the predicted lateral error and predicted heading error after delay compensation prediction, the initial lateral deviation of the foresight point is solved according to the pure tracking algorithm rules. The feedforward correction module is used to correct the initial lateral deviation according to the feedforward compensation amount, obtain the final lateral deviation of the forward view point, and solve the target steering angle based on the final lateral deviation of the forward view point. The control module is used to transmit the target steering angle to the agricultural machinery steering mechanism so that the agricultural machinery travels along the target trajectory curve.

9. An electronic device, characterized in that, include: At least one processor; as well as A memory communicatively connected to the at least one processor; wherein, The memory stores a computer program that can be executed by the at least one processor to enable the at least one processor to perform the method of any one of claims 1-7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions that are used to cause a processor to perform the method of any one of claims 1-7.

Images