Agricultural Machinery Trajectory Tracking Control Method and System Based on Implement Operation Position and Heading

Abstract

This invention discloses a method and system for tracking and controlling agricultural machinery trajectory based on the working position and heading of agricultural implements. The method includes: S1, setting parameters; S2, receiving GNSS position, heading, and speed information in real time, calculating the position, heading angle, and speed of the agricultural machinery, and calculating the position and heading angle of the implement based on the geometric relationship between the agricultural machinery and the implement; S3, generating a planned trajectory for the implement based on the position and heading angle of the implement and the agricultural machinery trajectory generation method; S4, calculating the optimal pre-aiming distance for correcting the agricultural machinery's deviation using a pre-aiming distance optimization model, based on farmland operation quality standard parameters, the position and heading angle of the implement, and the planned trajectory of the implement; and calculating the front wheel angle of the agricultural machinery; S5, driving the agricultural machinery to perform trajectory tracking based on the front wheel angle, and repeatedly executing S2 to S4 until manual intervention in trajectory tracking control or the end of the operation. This invention solves the problems of insufficient control precision, lack of multi-objective constraints, and poor adaptability in existing technologies for agricultural implements.

Description

Technical Field

[0001] This invention relates to the field of agricultural machinery automatic driving application technology, and in particular to a method and system for agricultural machinery trajectory tracking control based on the heading of the agricultural implement's operating position. Background Technology

[0002] Agriculture is an important component of the national economy. In modern agricultural production, the use of agricultural machinery has become increasingly widespread. However, traditional agricultural machinery operation requires manual operation by the driver, which presents problems and risks such as high workload, poor control precision, low production efficiency, and even human error.

[0003] With the completion of the BeiDou global system and the maturity and promotion of application technologies such as "BeiDou+" and "+BeiDou", as well as the support of relevant policies such as the inclusion of agricultural machinery automatic driving systems in agricultural machinery subsidies, the use of agricultural machinery with automatic driving functions has become increasingly common, bringing about tremendous changes to agricultural production.

[0004] However, in existing technologies, agricultural machinery automatic driving systems mostly use a combined inertial navigation system (INS) of GNSS and an inertial system to collect the position of the agricultural machinery, and calculate the wheel angle based on the deviation between the position of the agricultural machinery and the planned curve to achieve lateral control of the agricultural machinery. This type of method does not take into account that what actually needs to be controlled is the working position of the implement, not the position of the agricultural machinery, and cannot fully ensure the straightness requirements of the implement. Moreover, with the application of large agricultural implements, this error has become more prominent. At the same time, this type of method does not consider the requirements of plant spacing. For example, a seeder needs to control both the straightness of the seeding row and the plant spacing simultaneously, which cannot fully ensure the quality requirements of all field operations.

[0005] There are also methods that involve installing a GPS receiver antenna at the center of the implement to measure its position. However, this only measures the position of the implement's center and does not specifically address the calculation of the implement's operational points or the implementation of trajectory tracking control algorithms. In particular, it does not incorporate the requirements for plant spacing into the control constraints. Summary of the Invention

[0006] The purpose of this invention is to provide a method and system for tracking and controlling the trajectory of agricultural machinery based on the working position and heading of agricultural implements. Under the premise of meeting the dual requirements of straightness and plant spacing, it can realize the accurate calculation of agricultural implement position, trajectory deviation calculation and wheel angle control, improve the effective accuracy of agricultural implement control and enhance the quality of field operations.

[0007] To achieve the above objectives, the present invention provides a method for tracking and controlling the trajectory of agricultural machinery based on the heading of the implement's operating position, comprising:

[0008] S1, Setting parameters: including agricultural machinery geometric parameters, agricultural implement geometric parameters, GNSS antenna installation geometric parameters, agricultural machinery operation trajectory generation method and farmland operation quality standard parameters;

[0009] S2 receives GNSS position, heading, and speed information in real time, calculates the position, heading angle, and speed of the agricultural machinery, and calculates the position and heading angle of the agricultural implements based on the geometric relationship between the agricultural machinery and the implements.

[0010] S3 generates the planned trajectory of agricultural implements based on the position, heading angle, and agricultural machinery operation trajectory generation method of agricultural implements;

[0011] S4. Based on the farmland operation quality standard parameters, the position and heading angle of the implements, and the planned trajectory of the implements, the optimal aiming distance for correcting the machine's deviation is calculated using the aiming distance optimization model. Based on the pre-aiming distance Calculate the front wheel angle of the agricultural machinery;

[0012] The aiming distance optimization model includes:

[0013] a. Objective function , expressed as equation (8):

[0014] (8)

[0015] b. Constraints, including equations (9) and (10):

[0016] (9)

[0017] (10)

[0018] in, The weighting is for the lateral correction speed. This is the distance from the rear axle of the agricultural machinery to the geometric center of the implement. The angle between the line connecting the geometric center point of the rear wheel axle of the agricultural machinery to the aiming point and the axis of the agricultural machinery. For the forward speed of agricultural machinery, For the width of the farm implement, The deviation from the planned trajectory of farm implements. Let β be the heading angle of the agricultural machinery, and β be the angle between the direction of motion of the geometric center of the implement's working surface and the axis RS of the agricultural machinery. To control the cycle of command output, The allowable deviation for straightness in agricultural machinery operation. This refers to the velocity deviation along the axis of the agricultural implement caused by the lateral sway of the implement. Plant spacing, This refers to the allowable deviation in plant spacing;

[0019] S5, based on the front wheel rotation angle, drives the agricultural machinery to perform trajectory tracking, and repeats S2 to S4 until manual intervention is required to control the trajectory tracking or the operation ends.

[0020] Furthermore, based on the pre-aiming distance Calculate the front wheel angle of agricultural machinery The formula is equation (11):

[0021] (11)

[0022] in, This refers to the wheelbase between the front and rear axles of the agricultural machinery. Satisfying equation (12):

[0023] (12)

[0024] in, This refers to the deviation of the vehicle's rear axle center from the planned trajectory.

[0025] Furthermore, front wheel steering angle Further combine with the heading angle of agricultural implements The revised calculation formula is (14):

[0026] (14)

[0027] in, The deviation of the farm implement from the planned trajectory The distance between the geometric center point of the rear wheel axle of agricultural machinery 1 and the center point of the implement.

[0028] Furthermore, in S2, the position of the implement is determined by the coordinates of its geometric center point ( , Description, coordinates ( , ) and the heading angle of agricultural implements Through 1 Calculation yielded:

[0029] (1)

[0030] In the formula, ( , The latitude and longitude of the geometric center point of the rear wheel axle of the agricultural machinery are converted into the projected coordinates on the projection plane.

[0031] Furthermore, the main antenna of the GNSS is installed at the geometric center of the rear wheel axle of the agricultural machinery.

[0032] Furthermore, during the cyclic execution of S5, the allowable deviation in plant spacing is monitored in real time. And by adjusting the front wheel steering angle Ensure that equation (10) is satisfied, and Satisfying equation (7):

[0033] (7).

[0034] The present invention also provides an agricultural machinery trajectory tracking control system based on the heading of the agricultural implement's operating position, comprising:

[0035] The parameter setting unit is used to set parameters, including agricultural machinery geometric parameters, agricultural implement geometric parameters, GNSS antenna installation geometric parameters, agricultural machinery operation trajectory generation method, and farmland operation quality standard parameters.

[0036] The implement status parameter acquisition unit is used to receive GNSS position, heading and speed information in real time, calculate the position, heading angle and speed of the agricultural machinery, and calculate the position and heading angle of the implement based on the geometric relationship between the agricultural machinery and the implement;

[0037] The implement trajectory generation unit is used to generate implement planning trajectories based on the implement's position, heading angle, and agricultural machinery operation trajectory generation method.

[0038] The agricultural machinery front wheel steering angle calculation unit is used to calculate the optimal pre-aiming distance for correcting the agricultural machinery's deviation based on farmland operation quality standard parameters, the position and heading angle of the implements, and the planned trajectory of the implements, using a pre-aiming distance optimization model. Based on the pre-aiming distance Calculate the front wheel angle of the agricultural machinery;

[0039] The aiming distance optimization model includes:

[0040] a. Objective function , expressed as equation (8):

[0041] (8)

[0042] b. Constraints, including equations (9) and (10):

[0043] (9)

[0044] (10)

[0045] in, The weighting is for the lateral correction speed. This is the distance from the rear axle of the agricultural machinery to the geometric center of the implement. The angle between the line connecting the geometric center point of the rear wheel axle of the agricultural machinery to the aiming point and the axis of the agricultural machinery. For the forward speed of agricultural machinery, For the width of the farm implement, The deviation from the planned trajectory of farm implements. Let β be the heading angle of the agricultural machinery, and β be the angle between the direction of motion of the geometric center of the implement's working surface and the axis RS of the agricultural machinery. To control the cycle of command output, The allowable deviation for straightness in agricultural machinery operation. This refers to the velocity deviation along the axis of the agricultural implement caused by the lateral sway of the implement. Plant spacing, This refers to the allowable deviation in plant spacing;

[0046] The trajectory tracking unit is used to drive the agricultural machinery to track its trajectory based on the front wheel rotation angle.

[0047] Furthermore, based on the pre-aiming distance Calculate the front wheel angle of agricultural machinery The formula is equation (11):

[0048] (11)

[0049] in, This refers to the wheelbase between the front and rear axles of the agricultural machinery. Satisfying equation (12):

[0050] (12)

[0051] in, This refers to the deviation of the vehicle's rear axle center from the planned trajectory.

[0052] Furthermore, front wheel steering angle Further combine with the heading angle of agricultural implements The revised calculation formula is (14):

[0053] (14)

[0054] in, The deviation of the farm implement from the planned trajectory The distance between the geometric center point of the rear wheel axle of agricultural machinery 1 and the center point of the implement.

[0055] Furthermore, the allowable deviation of plant spacing is monitored in real time during the loop execution of the trajectory tracking unit. And by adjusting the front wheel steering angle Ensure that equation (10) is satisfied, and Satisfying equation (7):

[0056] (7).

[0057] The present invention has the following advantages due to the adoption of the above technical solutions:

[0058] 1. Precise control of implement operation position and heading: By calculating the geometric relationship between the agricultural machinery and implements in real time and combining GNSS data to dynamically correct the position and heading angle of the implements, it effectively solves the problem of implement straightness deviation caused by relying solely on the position of the agricultural machinery in traditional methods. It is especially suitable for large-scale agricultural implement operation scenarios and significantly improves the accuracy of operation trajectory tracking.

[0059] 2. Dual Requirements Collaborative Optimization: Through the pre-aiming distance optimization model, the allowable deviation of the straightness of the farm implement and the allowable deviation of the plant spacing are simultaneously included in the constraints. The deviation of the lateral correction speed and the longitudinal speed are balanced in the objective function to ensure that the sowing, tillage and other operations meet the field quality standards of straightness and plant spacing at the same time.

[0060] 3. Dynamic adaptive correction: The front wheel angle is adjusted based on the optimal pre-aiming distance calculated in real time, and a secondary correction is made in combination with the heading angle of the implement. This can quickly respond to changes in the movement state of the agricultural machinery, reduce error accumulation, and improve the robustness of the system.

[0061] 4. Enhanced versatility and flexibility: By parameterizing the geometric parameters of agricultural machinery and implements, it can adapt to different models of implements and operating scenarios, eliminating the need for additional sensors on the implements and reducing system deployment costs.

[0062] 5. Improved efficiency through automation: By monitoring deviations and adjusting steering in real time through closed-loop control, the need for manual intervention is reduced, enabling high-precision continuous operation in complex farmland environments and significantly improving production efficiency and operational consistency.

[0063] The above-mentioned beneficial technical effects have jointly solved the problems of insufficient control precision, lack of multi-objective constraints and poor adaptability of existing agricultural implements, and provided efficient and reliable technical support for the intelligentization of modern agricultural equipment. Attached Figure Description

[0064] Figure 1 The flowchart illustrates a method for tracking and controlling the trajectory of agricultural machinery based on agricultural implements as the position and heading, as provided by the present invention.

[0065] Figure 2 A schematic diagram of the geometric and kinematic parameters of the agricultural machinery, implements and GNSS installation provided for this invention.

[0066] Figure 3 A schematic diagram of the error tracking model for agricultural machinery and implements provided by the present invention. Detailed Implementation

[0067] In the accompanying drawings, the same or similar reference numerals are used to denote the same or similar elements or elements having the same or similar functions. The embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0068] like Figure 1As shown, the agricultural machinery trajectory tracking and control method based on the heading of the agricultural implement's operating position in this embodiment includes:

[0069] S1, setting parameters: including agricultural machinery geometric parameters, agricultural implement geometric parameters, GNSS antenna installation geometric parameters, agricultural machinery operation trajectory generation method, and farmland operation quality standard parameters.

[0070] Agricultural machinery geometric parameters include the wheelbase between the front and rear axles. .

[0071] The geometric parameters of agricultural implements include the perpendicular distance from the implement connector pin to the implement's discharge port or axle, the implement's wheel track, and the distance from the geometric center point of the rear axle of the agricultural machinery to the geometric center point of the implement. and the width of farm tools .

[0072] The GNSS antenna installation geometry parameters include the longitudinal projection distance from the GNSS antenna to the geometric center point of the rear wheel axle of the agricultural machinery.

[0073] Agricultural machinery operation trajectories and trajectory generation methods include generating trajectories using AB lines, translating based on the curve of the previous operation, or importing from historical operation maps.

[0074] The quality standard parameters for farmland operations include the allowable deviation of the straightness of agricultural machinery operations. Plant spacing and allowable deviation of plant spacing .

[0075] S2 receives GNSS position, heading, and speed information in real time, calculates the position, heading angle, and speed of the agricultural machinery, and calculates the position and heading angle of the agricultural implements based on the geometric relationship between the agricultural machinery and the implements.

[0076] The latitude and longitude information of GNSS is converted into plane coordinates using a coordinate projection algorithm. The projection method used can be the commonly used Gaussian plane projection or transverse Mercator projection.

[0077] S3 generates the planned trajectory of agricultural implements based on the position, heading angle, and agricultural machinery operation trajectory generation method of agricultural implements.

[0078] The planned trajectory of agricultural implements can be achieved using common path planning methods. For example, in the AB line mode, the trajectory is extrapolated from the AB line position, heading angle, implement width, and current position obtained when the implement is set up; in the translation mode based on the previous operation curve, the translation is calculated based on the implement width and the relationship between the curve position and angle of the previous operation; in the historical operation map import mode, the nearest point is found in the historical trajectory map based on the implement position, and the operation curve is selected.

[0079] S4. Based on the farmland operation quality standard parameters, the position and heading angle of the implements, and the planned trajectory of the implements, the optimal aiming distance for correcting the machine's deviation is calculated using the aiming distance optimization model. Based on the pre-aiming distance Calculate the front wheel angle of the agricultural machinery.

[0080] S5, based on the front wheel rotation angle, drives the agricultural machinery to perform trajectory tracking, and repeats S2 to S4 until manual intervention is required to control the trajectory tracking or the operation ends.

[0081] Figure 2 This is a schematic diagram illustrating the installation geometry and kinematic parameters of the agricultural machinery 1, implement 2, and GNSS 3 provided by the present invention. Point F is the geometric center of the front axle of agricultural machinery 1, point R is the geometric center of the rear axle of agricultural machinery 1, point C is the geometric center of the connection between agricultural machinery 1 and implement 2, point T is the geometric center of the working surface of implement 2, and points A and B are the two end points of implement 2, respectively. The antenna of GNSS 3 is installed along the central axis of agricultural machinery 1, and point R is designated as the main antenna installation point of GNSS 3, and point S is designated as the lateral auxiliary antenna installation point of GNSS 3. If the main antenna of GNSS 3 is not convenient to install at point R, it can be moved forward or backward according to actual conditions. Relevant position, speed, and other parameters can be calculated using simple geometric relationships, which will not be elaborated further.

[0082] Figure 3 A schematic diagram of the error tracking model for agricultural machinery and implements provided by the present invention.

[0083] Since the reference line for agricultural machinery planning and operation is mostly a straight line or approximately a straight line over a short distance, a right-handed coordinate system is established with the reference line as the x-direction, the projection point of the rear wheel axle on the x-axis as the origin, and the front left of the vehicle as the right-hand coordinate system to determine the y-axis and obtain the XY plane.

[0084] Based on the latitude and longitude of point R output by GNSS3 and the heading angle information of the SR baseline, the latitude and longitude of point R are converted into projected coordinates on the projection plane. , The heading angle of the SR baseline is converted to the heading angle of agricultural machinery 1. .

[0085] The coordinates of the center point T of the farm implement are ( , ), heading angle is .

[0086] The calculation model for the center point T of the agricultural implement is shown in the following formula (1):

[0087] (1)

[0088] During production operations, the rear wheels of agricultural machinery 1 generally do not skid and have no lateral velocity, therefore The forward speed of agricultural machinery 1 β is the pre-aiming distance for correcting the deviation of agricultural machinery 1, and β is the angle between the direction of movement of the geometric center of the implement's working surface and the vehicle's axis. The turning radius of the vehicle's rear wheels. The angle between the line connecting the geometric center point of the rear wheel axle to the aiming point and the vehicle's axis.

[0089] Based on the trigonometric relationship, we can obtain equations (2) and (3):

[0090] (2)

[0091] (3)

[0092] Combining equations (2) and (3), we can obtain equation (4):

[0093] (4)

[0094] According to kinematic relationships Let be the rotational angular velocity of agricultural machinery 1. The speed of the center T of the rear wheel axle of agricultural machinery 1. The pre-aiming distance is the distance between the rear axle center of the vehicle and the speed deviation along the axis of the agricultural implement 1 caused by the lateral sway of the agricultural implement 1 at both ends AB. for:

[0095] (5)

[0096] (6)

[0097] Combining the above formula, we can obtain:

[0098] (7)

[0099] In equation (4), β reflects the correction speed of lateral deviation, and this value should be as large as possible. In equation (7) This reflects the error in longitudinal jitter, and this value should be as small as possible. Therefore, when setting the weight of the lateral correction speed... Then, establish the objective function. :

[0100] (8)

[0101] At the same time, in order to ensure the deviation of farm implement 2 from the planned trajectory During the correction process, the straightness deviation shall not exceed the allowable deviation. To control the cycle of command output, the following inequality (9) must be satisfied:

[0102] (9)

[0103] To ensure that the plant spacing deviation is within the control range, the plant spacing... and allowable deviation of plant spacing Equation (10) must be satisfied:

[0104] (10)

[0105] Based on the objective function and the two constraints mentioned above, the optimal aiming distance can be calculated. .

[0106] Obtain the pre-aiming distance Then, the front wheel steering angle can be calculated according to the pure tracking calculation formula (11). .

[0107] (11)

[0108] Let the deviation of the vehicle's rear axle center from the planned trajectory be denoted by the triangle relationship:

[0109] (12)

[0110] (13)

[0111] In one embodiment, the front wheel steering angle Further combine with the heading angle of agricultural implements The revised calculation formula is (14):

[0112] (14)

[0113] In the above embodiments, based on the pre-aiming distance Calculate the front wheel angle of agricultural machinery This method can also be implemented using other existing methods, which will not be elaborated here.

[0114] This invention also provides a tracking and control system for agricultural machinery based on the heading of the implement's operating position, which includes: a parameter setting unit, an implement status parameter acquisition unit, an implement trajectory generation unit, an agricultural machinery front wheel angle calculation unit, and a trajectory tracking unit.

[0115] The parameter setting unit is used to set parameters, including agricultural machinery geometric parameters, agricultural implement geometric parameters, GNSS antenna installation geometric parameters, agricultural machinery operation trajectory generation method, and farmland operation quality standard parameters.

[0116] The implement status parameter acquisition unit is used to receive GNSS position, heading and speed information in real time, calculate the position, heading angle and speed of the agricultural machinery, and calculate the position and heading angle of the implement based on the geometric relationship between the agricultural machinery and the implement.

[0117] The implement trajectory generation unit is used to generate implement planning trajectories based on the implement's position, heading angle, and agricultural machinery operation trajectory generation method.

[0118] The agricultural machinery front wheel steering angle calculation unit is used to calculate the optimal pre-aiming distance for correcting the agricultural machinery's deviation based on farmland operation quality standard parameters, the position and heading angle of the implements, and the planned trajectory of the implements, using a pre-aiming distance optimization model. Based on the pre-aiming distance Calculate the front wheel angle of the agricultural machinery.

[0119] The aiming distance optimization model includes an objective function. , which is represented by equation (8).

[0120] The aiming distance optimization model also includes constraints, including equations (9) and (10).

[0121] The trajectory tracking unit is used to drive the agricultural machinery to track its trajectory based on the front wheel rotation angle.

[0122] In one embodiment, the allowable deviation of plant spacing is monitored in real time during the cyclic execution of the trajectory tracking unit. And by adjusting the front wheel steering angle Ensure that equation (10) is satisfied, and Satisfy equation (7).

[0123] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them. Those skilled in the art should understand that modifications can be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for tracking and controlling the trajectory of agricultural machinery based on the heading of the implement's operating position, characterized in that, include: S1, setting parameters: including agricultural machinery geometric parameters, agricultural implement geometric parameters, GNSS antenna installation geometric parameters, agricultural machinery operation trajectory generation method and farmland operation quality standard parameters; S2 receives GNSS position, heading, and speed information in real time, calculates the position, heading angle, and speed of the agricultural machinery, and calculates the position and heading angle of the agricultural implements based on the geometric relationship between the agricultural machinery and the implements. S3 generates the planned trajectory of agricultural implements based on the position, heading angle, and agricultural machinery operation trajectory generation method of agricultural implements; S4. Based on the farmland operation quality standard parameters, the position and heading angle of the implements, and the planned trajectory of the implements, the optimal aiming distance for correcting the machine's deviation is calculated using the aiming distance optimization model. Based on the pre-aiming distance Calculate the front wheel angle of the agricultural machinery; The aiming distance optimization model includes: a. Objective function , expressed as equation (8): （8） b. Constraints, including equations (9) and (10): （9） （10） in, The weighting is for the lateral correction speed. This is the distance from the rear axle of the agricultural machinery to the geometric center of the implement. The angle between the line connecting the geometric center point of the rear wheel axle of the agricultural machinery to the aiming point and the axis of the agricultural machinery. For the forward speed of agricultural machinery, For the width of the farm implement, The deviation from the planned trajectory of farm implements. Let β be the heading angle of the agricultural machinery, and β be the angle between the direction of motion of the geometric center of the implement's working surface and the axis RS of the agricultural machinery. To control the cycle of command output, The allowable deviation for straightness in agricultural machinery operation. This refers to the velocity deviation along the axis of the agricultural implement caused by the lateral sway of the implement. Plant spacing, This refers to the allowable deviation in plant spacing; S5, based on the front wheel rotation angle, drives the agricultural machinery to perform trajectory tracking, and repeats S2 to S4 until manual intervention is required to control the trajectory tracking or the operation ends.

2. The agricultural machinery trajectory tracking and control method based on the heading of the agricultural implement's operating position as described in claim 1, characterized in that, Based on pre-aiming distance Calculate the front wheel angle of agricultural machinery The formula is (11): （11） in, This refers to the wheelbase between the front and rear axles of the agricultural machinery. Satisfying equation (12): （12） in, This refers to the deviation of the vehicle's rear axle center from the planned trajectory.

3. The agricultural machinery trajectory tracking and control method based on the heading of the implement's operating position as described in claim 2, characterized in that, Front wheel steering angle Further combine with the heading angle of agricultural implements The revised calculation formula is (14): （14） in, The deviation of the farm implement from the planned trajectory The distance between the geometric center point of the rear wheel axle of agricultural machinery 1 and the center point of the implement.

4. The agricultural machinery trajectory tracking and control method based on the heading of the agricultural implement's operating position as described in any one of claims 1-3, characterized in that, In S2, the position of the farm implement is determined by the coordinates of its geometric center point. , Description, coordinates ( , ) and the heading angle of agricultural implements Through 1 Calculation yielded: （1） In the formula, ( , The latitude and longitude of the geometric center point of the rear wheel axle of the agricultural machinery are converted into the projected coordinates on the projection plane.

5. The agricultural machinery trajectory tracking and control method based on the heading of the implement's operating position as described in claim 4, characterized in that, The main antenna of the GNSS is installed at the geometric center of the rear wheel axle of the agricultural machinery.

6. The agricultural machinery trajectory tracking and control method based on the heading of the implement's operating position as described in claim 4, characterized in that, During the cyclic execution of S5, the allowable deviation of plant spacing is monitored in real time. And by adjusting the front wheel steering angle Ensure that equation (10) is satisfied, and Satisfying equation (7): （7）。 7. A trajectory tracking and control system for agricultural machinery based on the heading of the implement's operating position, characterized in that, include: The parameter setting unit is used to set parameters, including agricultural machinery geometric parameters, agricultural implement geometric parameters, GNSS antenna installation geometric parameters, agricultural machinery operation trajectory generation method, and farmland operation quality standard parameters. The implement status parameter acquisition unit is used to receive GNSS position, heading and speed information in real time, calculate the position, heading angle and speed of the agricultural machinery, and calculate the position and heading angle of the implement based on the geometric relationship between the agricultural machinery and the implement; The implement trajectory generation unit is used to generate implement planning trajectories based on the implement's position, heading angle, and agricultural machinery operation trajectory generation method. The agricultural machinery front wheel steering angle calculation unit is used to calculate the optimal pre-aiming distance for correcting the agricultural machinery's deviation based on farmland operation quality standard parameters, the position and heading angle of the implements, and the planned trajectory of the implements, using a pre-aiming distance optimization model. Based on the pre-aiming distance Calculate the front wheel angle of the agricultural machinery; The aiming distance optimization model includes: a. Objective function , expressed as equation (8): （8） b. Constraints, including equations (9) and (10): （9） （10） in, The weighting is for the lateral correction speed. This is the distance from the rear axle of the agricultural machinery to the geometric center of the implement. The angle between the line connecting the geometric center point of the rear wheel axle of the agricultural machinery to the aiming point and the axis of the agricultural machinery. For the forward speed of agricultural machinery, For the width of the farm implement, The deviation from the planned trajectory of farm implements. Let β be the heading angle of the agricultural machinery, and β be the angle between the direction of motion of the geometric center of the implement's working surface and the axis RS of the agricultural machinery. To control the cycle of command output, The allowable deviation for straightness in agricultural machinery operation. This refers to the velocity deviation along the axis of the agricultural implement caused by the lateral sway of the implement. Plant spacing, This refers to the allowable deviation in plant spacing; The trajectory tracking unit is used to drive the agricultural machinery to track its trajectory based on the front wheel rotation angle.

8. The agricultural machinery trajectory tracking control system based on the heading of the agricultural implement's operating position as described in claim 7, characterized in that, Based on pre-aiming distance Calculate the front wheel angle of agricultural machinery The formula is (11): （11） in, This refers to the wheelbase between the front and rear axles of the agricultural machinery. Satisfying equation (12): （12） in, This refers to the deviation of the vehicle's rear axle center from the planned trajectory.

9. The agricultural machinery trajectory tracking control system based on the heading of the agricultural implement's operating position as described in claim 8, characterized in that, Front wheel steering angle Further combine with the heading angle of agricultural implements The revised calculation formula is (14): （14） in, The deviation of the farm implement from the planned trajectory The distance between the geometric center point of the rear wheel axle of agricultural machinery 1 and the center point of the implement.

10. The agricultural machinery trajectory tracking control system based on the heading of the agricultural implement's operating position as described in any one of claims 7-9, characterized in that, The trajectory tracking unit monitors the allowable deviation of plant spacing in real time during cyclic execution. And by adjusting the front wheel steering angle Ensure that equation (10) is satisfied, and Satisfying equation (7): （7）。

Images