An optimal path planning and tracking control method for agricultural machine automatic driving

Abstract

The application discloses an optimal path planning and tracking control method for agricultural machine automatic driving, determines a head turning mode according to the interval of two adjacent work rows and the cross-row interval number of the agricultural machine, respectively constructs a head turning time cost function containing turning subsection motion speed and acceleration and deceleration parameters, generates an in-row work time cost function containing in-row acceleration, uniform speed and in-row deceleration processes, forms a global work time cost model, and combines the global work time cost modeling link, the optimal work row sequence output link, the global reference trajectory generation link and the path tracking control link to control the agricultural machine to track and harvest along the global reference trajectory. The application can realize the exclusive demand of overall planning of agricultural machine precision row-by-row and head efficient row-changing and the motion constraint of a large chassis, and can also consider the smoothness of straight line work and the responsiveness of head turning, so that the problems of low intelligent harvesting work efficiency and unstable harvesting quality of non-regular land are effectively solved.

Description

Technical Field

[0001] This invention relates to the field of agricultural machinery driving path planning technology, and more specifically, to an optimal path planning and tracking control method for automatic agricultural machinery driving. Background Technology

[0002] Agricultural machinery such as cotton harvesters are important mechanical equipment used in various stages of agricultural production, including tillage, sowing, field management, harvesting, and transportation, to replace human and animal power, improve agricultural production efficiency, and reduce labor intensity.

[0003] However, existing path planning methods for automated agricultural machinery mostly rely on static geometric distance or fixed speed travel time as the core decision-making basis, without fully considering the dynamic motion characteristics of agricultural machinery during operation, such as acceleration at the beginning of a line, deceleration at the end of a line, turning at the edge of the field, and reversing. Regular paths lack flexibility in irregular plots, and heuristic algorithms are prone to getting stuck in local optima and unstable convergence, resulting in a mismatch between the planned path and the actual operating rhythm of agricultural machinery.

[0004] In addition, existing trajectory tracking control methods for agricultural machinery are mostly designed for general navigation scenarios, without taking into account the specific needs of precise row alignment and continuous and efficient row switching at the field ends, as well as the motion constraints of large chassis. They are difficult to balance the smoothness of straight-line operation and the responsiveness of turning at the field ends, resulting in low operation efficiency, fluctuating harvest quality, and inability to adapt to the intelligent harvesting needs of irregular plots.

[0005] Based on this, this application proposes an optimal path planning and tracking control method for automatic agricultural machinery driving. Summary of the Invention

[0006] This invention aims to overcome the problems of existing agricultural machinery automatic driving path planning based on static geometric distance or fixed speed and travel time, which does not take into account the dynamic motion characteristics of agricultural machinery operation, and the poor flexibility of regular paths to adapt to irregular plots. Heuristic algorithms are prone to getting trapped in local optima, resulting in a mismatch between the planned path and the actual operation rhythm. In addition, trajectory tracking is multi-directional to general navigation scenarios, and fails to take into account the specific needs of precise alignment of agricultural machinery and efficient row changing at the field edge, as well as the motion constraints of large chassis. It is difficult to balance straight-line smoothness and turning responsiveness, resulting in low operation efficiency, unstable harvesting quality, and inability to adapt to the intelligent harvesting needs of irregular plots.

[0007] To achieve the above objectives, this invention provides an optimal path planning and tracking control method for automatic agricultural machinery operation, comprising: The operation row map construction process involves obtaining the prior location information of the sowing rows in the plot to be harvested, mapping it to a unified coordinate system through coordinate transformation, and based on the operation width of the agricultural machinery, converting adjacent rows into single benchmark operation rows, and then expanding several operation rows in parallel based on the single benchmark operation rows to construct the operation row map. Global operation time cost modeling step: Based on the distance between two adjacent operation rows and the number of cross-row intervals of the agricultural machinery, the field turning mode is determined and field turning time cost functions containing the turning segment movement speed and acceleration and deceleration parameters are constructed respectively. Intra-row operation time cost functions containing the entry acceleration, constant speed and end deceleration processes are generated to form a global operation time cost model. The optimal job row sequence output stage: Set the alternation constraint of the job direction of two adjacent job rows, and based on the preset dynamic programming algorithm, combined with the global job time cost model, obtain the optimal job row harvesting sequence that minimizes the global cumulative job time; Global reference trajectory generation: Based on the optimal work row harvesting sequence, the global harvesting trajectory is divided into several straight segments within the work row, several straight segments with bow-shaped turns, and several circular arc segments with bow-shaped turns. The starting point, ending point, and travel time of each trajectory segment are constrained, as well as the boundary velocity and boundary acceleration corresponding to the starting point and ending point. Each trajectory segment is fitted according to a fifth-order polynomial to generate the global reference trajectory. Path tracking control: Construct a kinematic model of a rear-wheel steering two-wheeled vehicle, and combine it with a speed adaptive pure tracking algorithm to define a dynamic adjustment expression for the pre-aiming distance of each trajectory segment. Based on the fifth-order polynomial expression corresponding to each trajectory segment, calculate the trajectory tangential velocity and generate a longitudinal velocity specification by combining the preset forward look time. Output the longitudinal velocity specification and the pre-acquired current expected rear wheel angle to the control terminal of the agricultural machine to control the agricultural machine to track and harvest along the global reference trajectory.

[0008] Optionally, the global job time cost modeling step specifically includes: Calculate the lateral crossing distance between two adjacent work rows based on the spacing between two adjacent work rows and the number of cross-row intervals of the agricultural machinery; The turning pattern is determined to be either fishtail or bow shape based on the lateral change distance and the minimum turning radius of the agricultural machinery; The fishtail turning pattern is divided into a circular arc forward deceleration segment, a reverse straight segment, and a circular arc forward acceleration segment. The time cost function of the fishtail turning at the end of the road is calculated. The bow-shaped turning pattern is divided into a circular arc constant speed segment and an oblique straight segment. The time cost function of the bow-shaped turning at the end of the road is calculated. Calculate the travel distance and entry distance of the agricultural machinery in the fishtail turn mode and bow turn mode, obtain the equivalent length of each work row, and generate the in-row operation time cost function by combining the corresponding acceleration distance and deceleration distance. By combining the time cost functions for fishtail-shaped turning points, bow-shaped turning points, and in-row operation time cost functions, a global operation time cost model is formed.

[0009] Optionally, the process of dividing the fishtail-shaped turning pattern into an arc-shaped forward deceleration segment, a reverse straight segment, and an arc-shaped forward acceleration segment, and calculating the fishtail-shaped turning time cost function, and dividing the bow-shaped turning pattern into an arc-shaped constant speed segment and an oblique straight segment, and calculating the bow-shaped turning time cost function, specifically includes: Based on the trajectory geometry of the fishtail turning pattern and the turning speed variation law of agricultural machinery, the fishtail turning pattern is divided into a circular arc forward deceleration section, a reverse straight section, and a circular arc forward acceleration section. Based on the division of the forward deceleration section, the reverse straight section, and the forward acceleration section of the circular arc, combined with the lateral lane change distance... Calculate the time cost function for turning at the fishtail-shaped headland. : In the formula: It is represented as the angle between the land parcel boundary and the direction of the work line; This represents the speed of travel on the circular section at the edge of the field. Expressed as the speed of travel on a straight segment, and ; Expressed as acceleration on a straight line segment; Expressed as deceleration on a straight line segment; It is represented as the acceleration of a fishtail-shaped circular arc segment, and ,in, Represented as The corresponding radian value; It is represented as the deceleration of a fishtail-shaped circular arc segment, and ; Based on the trajectory geometry of the bow-shaped turning pattern and the turning speed variation law of agricultural machinery, the bow-shaped turning pattern is divided into a circular arc uniform speed segment and an oblique straight line segment. Based on the division of the circular arc uniform speed segment and the oblique straight line segment, combined with the lateral line break distance Calculate the turning time cost function of the bow-shaped headland. : In the formula: This represents the minimum turning radius of the agricultural machinery.

[0010] Optionally, the global operation time cost model is formed by combining the fishtail-shaped turning time cost function, the bow-shaped turning time cost function, and the in-row operation time cost function, specifically including: Combining the fishtail-shaped turning time cost function Bow-shaped turning time cost function and in-row operation time cost function To form a global operation time cost model : In the formula: Represented as the first One operation line; This is expressed as the headway turning time cost function between two adjacent work lines, and .

[0011] Optionally, the optimal job row order output stage specifically includes: Define each work line as either a forward work state or a reverse work state, and set an alternating work direction constraint for two adjacent work lines; Based on the Held-Karp dynamic programming algorithm, combined with the operation state function within the harvesting plot, the additional time cost function between two adjacent operation rows is calculated, and the dynamic programming recursive equation is obtained. Each work row is defined as an independent starting candidate. The cumulative time of the starting state is the in-row work time of the corresponding work row. The harvested work rows are traversed according to the dynamic programming recursive equation, and the state is updated iteratively based on the number of them. In each round, one work row is selected from the unharvested work rows and the new transfer cost is calculated to update the cumulative time. When all work rows are harvested, the harvesting sequence corresponding to the termination work row with the smallest cumulative time is selected as the optimal work row harvesting sequence.

[0012] Optionally, if each work row is defined as an independent starting candidate, then the cumulative time of the starting state is the in-row work time of the corresponding work row. The harvested work rows are traversed according to the dynamic programming recursive equation, and the state is updated iteratively based on their increasing number. In each round, one unharvested work row is selected to be added, and the new transfer cost is calculated to update the cumulative time. When all work rows have been harvested, the harvesting sequence corresponding to the terminating work row with the smallest cumulative time is selected as the optimal work row harvesting sequence, specifically including: Define each job line as an independent starting candidate, and set each starting job line as... Then its initial state cumulative time : In the formula: Indicates the initial working direction; This represents the in-row operation time of the starting job row; The harvested work rows are traversed according to the dynamic programming recursive equation, and the status is updated iteratively based on their quantity. In each round, one work row is selected from the unharvested work rows and added, and the new transfer cost is calculated and the cumulative time is updated. When all work lines have been collected, in all candidate termination work lines The work line with the minimum cumulative working time is selected as the termination work line, and the optimal work line harvesting sequence is obtained based on the work line harvesting sequence corresponding to the termination work line: In the formula: This represents the total number of rows in the operation. This represents the operational state function within the harvesting area.

[0013] Optionally, the global reference trajectory generation step specifically includes: Based on the optimal working row harvesting sequence and combined with the bow-shaped turn geometric model, the global harvesting trajectory is divided into several straight segments within the working row, several straight segments of bow-shaped turns, and several circular arc segments of bow-shaped turns. The starting point, ending point, and travel time of each trajectory segment are determined, as well as the boundary velocity and boundary acceleration corresponding to the starting point and ending point. For each trajectory segment, respectively in direction and The direction establishes a fifth-order polynomial: Based on the start and end positions, boundary velocities, boundary accelerations, and travel time of each trajectory segment, obtain... polynomial coefficients in the direction Formula for calculating polynomial coefficients in a direction; Based on the start and end points of the straight line segments in each work row, the normalized direction vector is obtained, and the end point of the acceleration segment and the start point of the deceleration segment are calculated according to the corresponding boundary velocity and boundary acceleration. These are then substituted into the polynomial coefficient calculation formula to obtain the corresponding fifth-order polynomial expression. Substituting the starting point, ending point, travel time, boundary velocity, and boundary acceleration corresponding to each bow-shaped U-turn straight segment into the polynomial coefficient calculation formula, we obtain the corresponding fifth-order polynomial expression. Substituting the starting point, ending point, travel time, boundary velocity, and boundary acceleration corresponding to each arc segment of the U-turn into the polynomial coefficient calculation formula, we obtain the corresponding fifth-order polynomial expression. Based on the optimal work row harvesting sequence, the straight segments within each work row, the straight segments of each bow-shaped turn, and the circular arc segments of each bow-shaped turn are sequentially spliced ​​together to generate a global reference trajectory.

[0014] Optionally, the step of sequentially splicing together the straight segments within each work row, the straight segments of each bow-shaped turn, and the circular arc segments of each bow-shaped turn based on the optimal work row harvesting sequence to generate a global reference trajectory specifically includes: Let the first The fifth-order polynomial expression for the trajectory segment is: Then the global reference trajectory for: In the formula: Indicates the total number of trajectory segments; And generate cumulative time series : Optionally, the path tracking control mechanism specifically includes: The kinematic model of a rear-wheel steering two-wheeled vehicle is constructed by pre-setting the coordinates of the center of the front wheel of the agricultural machinery in the world coordinate system, and combining its front and rear wheelbase, rear wheel angle, heading angle, driving speed and turning radius. Based on the length of the line connecting the origin and the aiming point of the agricultural machinery in its on-board coordinate system, the expression for its turning radius is defined, and the expected rear wheel turning angle is calculated. Based on the speed-adaptive pure tracking algorithm, a dynamic adjustment expression for the pre-aiming distance of each trajectory segment is defined; Based on the current position of the agricultural machinery, extract the fifth-order polynomial expression of the current trajectory segment, and combine it with the corresponding dynamic adjustment aiming distance expression to construct the forward aiming position along the current heading of the agricultural machinery; Extract the closest point on the global reference trajectory to the current position of the agricultural machinery, select the intersection point between it and the search line corresponding to the forward aiming position and define it as the target point, and calculate the current expected rear wheel angle of the agricultural machinery based on the target point; Based on the global time corresponding to the nearest point on the global reference trajectory where the current position of the agricultural machinery is located, and combined with the preset forward look time, the trajectory tangential velocity at the predicted reference point is calculated and a longitudinal velocity command is generated. The longitudinal velocity command and the current expected rear wheel rotation angle are both input into the chassis controller of the agricultural machinery to control the agricultural machinery to track and harvest along the global reference trajectory.

[0015] Optionally, the global time corresponding to the nearest point on the global reference trajectory based on the current position of the agricultural machinery, combined with a preset forward look time, is used to calculate the trajectory tangential velocity at the predicted reference point and generate a longitudinal velocity command. The longitudinal velocity command and the currently desired rear wheel angle are then input into the chassis controller of the agricultural machinery to control the machinery to track and harvest along the global reference trajectory. Specifically, this includes: Let the global time corresponding to the nearest point on the global reference trajectory to the current position of the agricultural machinery be . Combined with preset forward look time Obtain the reference time for longitudinal velocity calculation. : Among them, if reference time If the total duration exceeds the global reference trajectory, it is determined that the agricultural machinery is approaching the end of the global reference trajectory, and the reference time is then used. The trajectory tangential velocity at the corresponding predicted reference point is 0; Otherwise, set a reference time. Located in the Within the trajectory segment, and the first The start time of the trajectory segment is Then the first Local time within the trajectory segment for: For the first Taking the first derivative of the fifth-order polynomial expression of the trajectory segment, we obtain the predicted reference point at... direction and velocity components in the direction and The tangential velocity of the trajectory is obtained by synthesis. And generate longitudinal speed commands; The longitudinal speed command and the current desired rear wheel angle are both input into the chassis controller of the agricultural machine to control the agricultural machine to follow the global reference trajectory for harvesting.

[0016] Compared with the prior art, the present invention has the following beneficial effects: This invention proposes an optimal path planning and tracking control method for automated agricultural machinery. By constructing a work row map adapted to the width of the agricultural machinery's work area, it transforms prior information about sowing rows into basic work path units that conform to the operational logic of the machinery, providing a precise spatial reference for path planning on irregular plots. Furthermore, by constructing a global operation time cost model incorporating dynamic motion processes, it integrates dynamic features such as acceleration upon entering a row, deceleration at the end of a row, and segmented turning movements into path cost calculations, replacing the single decision-making basis of static geometric distance or fixed speed, ensuring the planned path aligns with the actual operational rhythm. Finally, by setting alternating operation directions... The system employs a pre-defined dynamic programming algorithm to solve for the optimal row sequence, minimizing the global operation time in irregular plots. This avoids the lack of flexibility in regular paths and the problem of heuristic algorithms getting trapped in local optima. Furthermore, by generating a global reference trajectory of the boundary motion state of constrained trajectory segments, and combining the kinematic model of a rear-wheel steering two-wheeled vehicle with the coordinated control of a speed-adaptive pure tracking algorithm, the system can comprehensively address the specific needs of precise row alignment and efficient row changing at the field edge, while also considering the motion constraints of the large chassis. This balances the smoothness of straight-line operation with the responsiveness of turning at the field edge, thereby effectively solving the problems of low efficiency and unstable harvesting quality in intelligent harvesting operations in irregular plots.

[0017] As can be seen from the above, the technical solution of the present invention can effectively solve the problems of existing agricultural machinery automatic driving path planning based on static geometric distance or fixed speed travel time, without considering the dynamic motion characteristics of agricultural machinery operation, and the poor flexibility of regular paths to adapt to irregular plots. Heuristic algorithms are prone to getting trapped in local optima, resulting in a mismatch between the planned path and the actual operation rhythm. In addition, trajectory tracking is multi-directional general navigation scenario, without taking into account the specific needs of precise alignment of agricultural machinery and efficient row changing at the field head, as well as the motion constraints of large chassis. It is difficult to balance straight-line smoothness and turning responsiveness, resulting in low operation efficiency, unstable harvesting quality, and inability to adapt to the intelligent harvesting needs of irregular plots.

[0018] Other features and advantages of the present invention will be described in detail in the following detailed description section. Attached Figure Description

[0019] Figure 1 This is a flowchart illustrating the optimal path planning and tracking control method for automated agricultural machinery driving according to an embodiment of the present invention. Figure 2 This is a schematic diagram of a simulated land parcel according to an embodiment of the present invention. Detailed Implementation

[0020] To enable those skilled in the art to more fully understand the technical solutions of the present invention, exemplary embodiments of the present invention will be described more comprehensively and in detail below with reference to the accompanying drawings. Obviously, the one or more embodiments of the present invention described below are merely one or more specific ways to implement the technical solutions of the present invention, and are not exhaustive. It should be understood that other ways belonging to a general inventive concept can be used to implement the technical solutions of the present invention, and should not be limited to the embodiments described exemplary. Based on one or more 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.

[0021] Reference Figure 1 The present invention provides an optimal path planning and tracking control method for automated agricultural machinery, comprising: Step S1: Operation row map construction: Obtain the prior location information of the sowing rows of the plot to be harvested, map it to a unified coordinate system through coordinate transformation, and based on the operation width of the agricultural machinery, convert adjacent rows into single benchmark operation rows, and expand several operation rows in parallel according to the single benchmark operation rows to construct the operation row map. Step S2: Global operation time cost modeling: Based on the distance between two adjacent operation rows and the number of row-crossing intervals of the agricultural machinery, determine the turning mode at the edge of the field and construct the turning time cost function at the edge of the field, which includes the turning segment speed and acceleration / deceleration parameters. Generate the in-row operation time cost function, which includes the entry acceleration, constant speed and deceleration process at the end of the row, to form a global operation time cost model. Step S3: Optimal job row sequence output: Set the alternation constraint of the job direction of two adjacent job rows, and based on the preset dynamic programming algorithm, combined with the global job time cost model, obtain the optimal job row harvesting sequence that minimizes the global cumulative job time; Step S4: Global reference trajectory generation: Based on the optimal work row harvesting sequence, the global harvesting trajectory is divided into several straight segments within the work row, several straight segments with bow-shaped turns, and several circular arc segments with bow-shaped turns. The starting point, ending point, and travel time of each trajectory segment are constrained, as well as the boundary velocity and boundary acceleration corresponding to the starting point and ending point. Each trajectory segment is fitted according to a fifth-order polynomial to generate a global reference trajectory. Step S5: Path tracking control: Construct a kinematic model of a rear-wheel steering two-wheeled vehicle, and combine it with a speed adaptive pure tracking algorithm to define a dynamic adjustment expression for the pre-aiming distance of each trajectory segment. Based on the fifth-order polynomial expression corresponding to each trajectory segment, calculate the trajectory tangential velocity and generate a longitudinal velocity specification by combining the preset forward look time. Output the longitudinal velocity specification and the pre-acquired current expected rear wheel angle to the control terminal of the agricultural machine to control the agricultural machine to track and harvest along the global reference trajectory.

[0022] This invention proposes an optimal path planning and tracking control method for automated agricultural machinery. By constructing a work row map adapted to the width of the agricultural machinery's work area, it transforms prior information about sowing rows into basic work path units that conform to the operational logic of the machinery, providing a precise spatial reference for path planning on irregular plots. Furthermore, by constructing a global operation time cost model incorporating dynamic motion processes, it integrates dynamic features such as acceleration upon entering a row, deceleration at the end of a row, and segmented turning movements into path cost calculations, replacing the single decision-making basis of static geometric distance or fixed speed, ensuring the planned path aligns with the actual operational rhythm. Finally, by setting alternating operation directions... The system employs a pre-defined dynamic programming algorithm to solve for the optimal row sequence, minimizing the global operation time in irregular plots. This avoids the lack of flexibility in regular paths and the problem of heuristic algorithms getting trapped in local optima. Furthermore, by generating a global reference trajectory of the boundary motion state of constrained trajectory segments, and combining the kinematic model of a rear-wheel steering two-wheeled vehicle with the coordinated control of a speed-adaptive pure tracking algorithm, the system can comprehensively address the specific needs of precise row alignment and efficient row changing at the field edge, while also considering the motion constraints of the large chassis. This balances the smoothness of straight-line operation with the responsiveness of turning at the field edge, thereby effectively solving the problems of low efficiency and unstable harvesting quality in intelligent harvesting operations in irregular plots.

[0023] In one embodiment, the job row map construction step S1 specifically includes: Step S11: Obtain prior location information including the spatial coordinates of the sowing rows of all plots to be harvested; Step S12: Map the prior position information to a unified coordinate system through translation and rotation transformations; Step S13: Based on the operating width of the agricultural machinery, merge adjacent rows into a single baseline operating row; Step S14: Extend several work lines in parallel from a single baseline work line, and record the position, length and spacing information of all work lines into a unified coordinate system to construct a work line map.

[0024] Specifically, the prior location information, including the spatial coordinates of the sowing rows of all plots to be harvested, is the spatial coordinate data of the sowing rows recorded and stored in real time during operation by precision sowing equipment, such as seeders with GNSS navigation systems, which is a general prior operational data retained in the agricultural production process.

[0025] The working width of agricultural machinery refers to the width of the working rows that the machinery can cover simultaneously during a single operation. The agricultural machinery in this embodiment is a six-row self-propelled cotton harvester, capable of harvesting six working rows simultaneously. Its specific structure and working principle are existing technologies, and will not be elaborated upon further here.

[0026] By mapping prior location information to a unified coordinate system and constructing a work row map, the spatial benchmark of the prior location information of all sowing rows can be unified, so as to form a complete spatial map of the work rows of the plot, including the spatial distribution and geometric dimensions of the work rows, and provide a benchmark spatial model for subsequent path planning.

[0027] In one embodiment, the global job time cost modeling step S2 specifically includes: Step S21: Calculate the lateral crossing distance between two adjacent work rows based on the spacing between two adjacent work rows and the number of cross-row intervals of the agricultural machinery; Step S22: Determine whether the turning pattern is fishtail or bow shape based on the lateral lane change distance and the minimum turning radius of the agricultural machinery; Step S23: Divide the fishtail turning pattern into an arc forward deceleration segment, a reverse straight segment, and an arc forward acceleration segment, calculate the fishtail turning time cost function, and divide the bow turning pattern into an arc constant speed segment and an oblique straight segment, calculate the bow turning time cost function. Step S24: Calculate the travel distance and entry distance of the agricultural machinery in the fishtail turn mode and bow turn mode, obtain the equivalent length of each work row, and generate the in-row operation time cost function by combining the corresponding acceleration distance and deceleration distance. Step S25: Combine the fishtail-shaped turning time cost function, the bow-shaped turning time cost function, and the in-row operation time cost function to form a global operation time cost model.

[0028] In one specific embodiment, in step S21, the lateral crossing distance between two adjacent work rows is calculated based on the spacing between two adjacent work rows and the number of row crossings of the agricultural machinery. This specifically includes: Let the distance between two adjacent work rows be . The number of row intervals for agricultural machinery is The horizontal wrapping distance between two adjacent work lines is... for: .

[0029] In one specific embodiment, in step S22, the turning pattern is determined to be either a fishtail shape or an arc shape based on the lateral lane change distance and the minimum turning radius of the agricultural machinery, specifically including: Let the minimum turning radius of the agricultural machinery be... ,but: when <2 The turning pattern is determined to be a fishtail shape; when 2 The turning pattern is determined to be an arc shape.

[0030] Specifically, by quantitatively matching the lateral turning distance with the minimum turning radius, a turning mode suitable for the field space is precisely selected, effectively avoiding problems such as insufficient space, increased reversing times, and discontinuous trajectory when large-sized agricultural machinery turns. The minimum turning radius is determined based on the agricultural machinery's structural parameters and steering system performance indicators, and is an inherent technical parameter calibrated at the factory.

[0031] In one specific embodiment, in step S23, the fishtail-shaped turning pattern is divided into an arc-shaped forward deceleration segment, a reverse straight segment, and an arc-shaped forward acceleration segment, and the fishtail-shaped turning time cost function is calculated. Similarly, the bow-shaped turning pattern is divided into an arc-shaped constant speed segment and an oblique straight segment, and the bow-shaped turning time cost function is calculated. Specifically, this includes: Step S231: Based on the trajectory geometry of the fishtail turning pattern and the turning speed variation law of the agricultural machinery, the fishtail turning pattern is divided into a circular arc forward deceleration section, a reverse straight section, and a circular arc forward acceleration section. Step S232: Based on the division of the circular forward deceleration segment, the reverse straight segment, and the circular forward acceleration segment, combined with the lateral lane change distance... Calculate the time cost function for turning at the fishtail-shaped headland. : In the formula: It is represented as the angle between the land parcel boundary and the direction of the work line; This represents the speed of travel on the circular section at the edge of the field. Expressed as the speed of travel on a straight segment, and ; Expressed as acceleration on a straight line segment; Expressed as deceleration on a straight line segment; It is represented as the acceleration of a fishtail-shaped circular arc segment, and ,in, Represented as The corresponding radian value; It is represented as the deceleration of a fishtail-shaped circular arc segment, and ; Step S233: Based on the trajectory geometry of the bow-shaped turning pattern and the turning speed variation law of the agricultural machinery, the bow-shaped turning pattern is divided into a circular arc uniform speed segment and an oblique straight line segment. Step S234: Based on the division of the circular arc uniform speed segment and the oblique straight line segment, combined with the lateral line break distance... Calculate the turning time cost function of the bow-shaped headland. : Specifically, the turning speed variation pattern of agricultural machinery is obtained by comprehensively considering the structural parameters of the agricultural machinery, such as the minimum turning radius and vehicle size; the performance of the power system, such as the acceleration and deceleration capabilities in straight sections / circular sections; and the actual harvesting operation specifications, such as the need for low-speed and smooth turning at the edge of the field to avoid damage to crops. It is determined by combining the factory calibration data of agricultural machinery, field operation measurement data, and industry operation standards.

[0032] By combining the geometric shape of the turning trajectory with the actual motion characteristics of the agricultural machinery for fine segmentation, the time cost of the two turning modes, namely fishtail and bow, can be accurately calculated. This achieves a high degree of matching between the turning time at the field end and the actual motion process of the agricultural machinery, effectively solving the error problem caused by estimating the turning time based solely on static geometric distance or fixed speed. In turn, it can provide accurate time cost basis for the global optimal operation sequence planning.

[0033] In one specific embodiment, in step S24, the travel distance and entry distance of the agricultural machinery in the fishtail turn mode and bow turn mode are calculated to obtain the equivalent length of each work row. Combined with the corresponding acceleration and deceleration distances, an in-row work time cost function is generated, specifically including: Step S241: Let the travel distance of the agricultural machinery in the reference reversing direction be... The entry distance is In the fishtail turn mode and bow turn mode, the travel distance is and the distance to the entry line is They are respectively: In the formula: It represents the horizontal distance from the end of the turn to the starting point of the next work row after the agricultural machinery completes the turn at the edge of the field; Among them, the reference row change direction indicates the preset standard direction in which agricultural machinery changes from the working row near the plot boundary to the working row away from the plot boundary; Step S242: Based on travel distance and the distance to the entry line is To obtain the distance conversion formula in the opposite direction to the baseline wrapping direction: In the formula: This is represented as the line break direction discrimination coefficient, and When the line break direction is the same as the reference line break direction; At this time, the line break direction is opposite to the reference line break direction; Step S243: Let the first... The equivalent length of each operation line is ,but: In the formula: This represents the number of rows in the operation; This represents the total number of rows in the operation. Step S244: Calculate the acceleration distance corresponding to each work line. and deceleration distance : In the formula: This indicates that agricultural machinery has entered the first stage. The speed of each operation line; This indicates that the agricultural machinery has left the first... The speed of each operation line; And calculate the first Uniform working distance within the work row : To generate the in-row job time cost function : Specifically, by clearly defining the travel and entry distances of agricultural machinery in fishtail and bow-shaped turning modes, the transition distances of the machinery when turning at the edge of the field are precisely quantified. Through a distance exchange formula, the adaptive adjustment of the entry and exit distances of the work rows under different changing directions is achieved. This ensures that the generated in-row operation time cost function perfectly matches the actual operation state of the agricultural machinery from starting from a standstill, transitioning through intermediate turns, to finally stopping. It accurately covers the complete dynamic process of entering the row for acceleration, uniform harvesting, and deceleration at the end of the row, avoiding the error of calculating time solely based on the original length of the work row. This provides more accurate in-row time parameters for the global operation time cost model, thereby making the planning of the optimal work row sequence more consistent with the actual operation rhythm of the agricultural machinery.

[0034] In one specific embodiment, in step S25, a global operation time cost model is formed by combining the fishtail-shaped turning time cost function, the bow-shaped turning time cost function, and the in-row operation time cost function, specifically including: Combining the fishtail-shaped turning time cost function Bow-shaped turning time cost function and in-row operation time cost function To form a global operation time cost model : In the formula: This is expressed as the headway turning time cost function between two adjacent work lines, and .

[0035] In one embodiment, the optimal job row sequence output step in step S3 specifically includes: Step S31: Define each work line as either a forward work state or a reverse work state, and set alternating work direction constraints for two adjacent work lines; Step S32: Based on the Held-Karp dynamic programming algorithm, combined with the operation state function within the mining area, calculate the additional time cost function between two adjacent operation rows, and obtain the dynamic programming recursive equation; Step S33: Define each work row as an independent starting candidate. The cumulative time of the starting state is the working time of the corresponding work row in the forward row. According to the dynamic programming recursive equation, traverse the harvested work rows and update the state iteratively based on their quantity. In each round, select one work row from the unharvested work rows to add and calculate the new transfer cost to update the cumulative time. When all work rows have been harvested, select the harvesting sequence corresponding to the termination work row with the smallest cumulative time as the optimal work row harvesting sequence.

[0036] Specifically, the forward operation state means that the agricultural machinery enters from the starting point of the operation row and exits from the ending point; the reverse operation state is the opposite. The operation direction of two adjacent operation rows is set to alternate, so that after the current operation row is completed, it can only connect to the entry point of the next operation row from its exit end, ensuring the continuous connection of the field path and avoiding the duplication or omission of operation rows.

[0037] In one specific embodiment, in step S32, based on the Held-Karp dynamic programming algorithm and combined with the operation state function within the harvesting plot, the additional time cost function between two adjacent operation rows is calculated, and the dynamic programming recursive equation is obtained, specifically including: Step S321: Let the operational state function within the harvesting plot be... ,in, This represents the set of harvested work rows. If the current path is the termination line of the job, then terminate the job line. The direction is: In the formula: Indicates the initial working direction; This represents the number of harvested rows currently being processed; This represents the alternating transition between forward and reverse operation states; Then the next harvesting row The direction is: Step S322: Calculate the first... The first and second job lines Additional time cost function between job lines : In the formula: This represents the pure turning time cost at the head of the work line during work line switching. Represented as the first The work line is in the direction The following is the in-line operation time; Step S323: Obtain the dynamic programming recurrence equation : Specifically, by calculating the termination line of the operation. The direction and the next harvesting row The direction is automatically alternated based on the number of harvested rows and the initial direction, without the need for additional manual setting of direction rules, ensuring continuous connection of turning paths at the field ends.

[0038] The additional time cost function combines the pure turning time at the edge of the field when switching work rows with the in-row work time of the next work row, so that the cost calculation of the recursive process is fully in line with the complete operation process of agricultural machinery of "turning and changing rows and harvesting in-row".

[0039] In one specific embodiment, in step S33, each work row is defined as an independent starting candidate, and the cumulative time of the starting state is the in-row work time of the corresponding work row. The harvested work rows are traversed according to the dynamic programming recursive equation, and the state is updated iteratively based on their increasing number. In each round, one unharvested work row is selected to be added, and the new transfer cost is calculated to update the cumulative time. When all work rows are harvested, the harvesting sequence corresponding to the terminating work row with the smallest cumulative time is selected as the optimal work row harvesting sequence, specifically including: Step S331: Define each job line as an independent starting candidate, and let each starting job line be... Then its initial state cumulative time : In the formula: This represents the in-row operation time of the starting job row; Step S332: Traverse the harvested work rows according to the dynamic programming recursive equation, and update the status iteratively based on their quantity. In each round, select one work row from the unharvested work rows to add and calculate the new transfer cost to update the cumulative time. Step S333: When all work lines have been collected, select all candidate termination work lines. The work line with the minimum cumulative working time is selected as the termination work line, and the optimal work line harvesting sequence is obtained based on the work line harvesting sequence corresponding to the termination work line: Specifically, this embodiment is based on the core logic of the Held-Karp dynamic programming algorithm. First, each work row is treated as an independent candidate for starting. The in-row operation time of a single work row is used as the initial cumulative time. Based on the iterative rule of increasing number of harvested work rows, one work row is selected from the unharvested work rows in each round. Combined with the dynamic programming recursive equation, it is added to the end of all possible harvested work row sequences, and the minimum cumulative time of the corresponding state is updated. After all work rows are included in the harvesting sequence, the cumulative total time corresponding to all candidate termination work rows is compared, and the sequence corresponding to the minimum value is selected as the optimal work row harvesting sequence. In this way, by exhaustively exploring the time cost of all possible combinations of work rows, the globally optimal solution is found.

[0040] Furthermore, by using dynamic programming recursive equations to traverse all harvested work rows as the terminating work row of the preceding path, it ensures that each step of the recursion can select the path with the shortest cumulative time, thereby avoiding deviations caused by calculating turns or in-row times separately. This addresses the problems of heuristic algorithms easily getting trapped in local optima and the poor flexibility of regular paths in adapting to irregular plots.

[0041] In one embodiment, the global reference trajectory generation step in step S4 specifically includes: Step S41: Based on the optimal work row harvesting sequence and combined with the bow-shaped turn geometric model, the global harvesting trajectory is divided into several straight segments within the work row, several bow-shaped turn straight segments, and several bow-shaped turn circular arc segments. The starting point, ending point, and travel time of each trajectory segment are determined, as well as the boundary velocity and boundary acceleration corresponding to the starting point and ending point. Step S42: For each trajectory segment, respectively in direction and The direction establishes a fifth-order polynomial: Step S43: Based on the start position, end position, boundary velocity, boundary acceleration, and travel time of each trajectory segment, obtain... polynomial coefficients in the direction Formula for calculating polynomial coefficients in a direction; Step S44: Based on the start and end points of the straight line segments in each work row, obtain the normalized direction vector, and calculate the end point of the acceleration segment and the start point of the deceleration segment according to the corresponding boundary velocity and boundary acceleration. Substitute these into the polynomial coefficient calculation formula to obtain the corresponding fifth-order polynomial expression. Step S45: Substitute the starting point, ending point, travel time, boundary velocity, and boundary acceleration corresponding to each bow-shaped U-turn straight segment into the polynomial coefficient calculation formula to obtain the corresponding fifth-order polynomial expression; Step S46: Substitute the starting point, ending point, travel time, boundary velocity, and boundary acceleration corresponding to each arc segment of the U-turn into the polynomial coefficient calculation formula to obtain the corresponding fifth-order polynomial expression; Step S47: Based on the optimal work row harvesting sequence, the straight segments, the straight segments of each bow-shaped turn, and the circular arc segments of each bow-shaped turn within each work row are sequentially spliced ​​together to generate a global reference trajectory.

[0042] In one specific embodiment, in step S41, based on the optimal work row harvesting sequence and combined with the bow-shaped turn geometric model, the global harvesting trajectory is divided into several straight segments within the work row, several straight segments of bow-shaped turns, and several circular arc segments of bow-shaped turns. The starting point, ending point, and travel time of each trajectory segment, as well as the boundary velocity and boundary acceleration corresponding to the starting point and ending point, are determined. Specifically, this includes: Step S411: Based on the optimal working row harvesting sequence and combined with the bow-shaped turn geometric model, the global harvesting trajectory is divided into several straight segments within the working row, several straight segments of bow-shaped turns, and several circular arc segments of bow-shaped turns. Step S412: Define the start and end points of each straight segment within the work row according to the actual start and end points of each segment. Calculate the corresponding travel time based on the work time cost function within the row. Define the start boundary speed and end boundary speed according to the initial speed at the start of the row and the deceleration target speed at the end of the row. Define the start boundary acceleration and end boundary acceleration of the straight-line driving acceleration performance parameters and the straight-line driving deceleration performance parameters of the agricultural machinery respectively. Step S413: Define the start and end points of each arc-shaped U-turn straight segment at the end of the current work line and the beginning of the next work line, respectively. Calculate the corresponding travel time based on the turning time cost function. Define the start boundary velocity and end boundary velocity based on the tangential velocity at the end of the arc segment and the initial velocity of the next line, respectively. The start boundary acceleration and end boundary acceleration are both defined as 0. Step S414: Define the starting point and ending point of each arc segment and the starting point of the straight segment of the arc U-turn, respectively. Calculate the arc length based on the arc radius and the central angle radian, and calculate the travel time in combination with the travel speed of the arc segment at the beginning of the road. Define the starting point boundary speed and the ending point boundary speed according to the tangential direction of the starting point and the ending point, respectively, and define the starting point boundary acceleration and the ending point boundary acceleration as 0.

[0043] Specifically, this embodiment first uses the optimal harvesting sequence of the harvesting rows to determine the harvesting order of the harvesting rows. Then, considering the connection requirements between adjacent harvesting rows, it calls upon the existing bow-shaped turning geometry model to pre-calculate a set of fixed-shape turning path combinations based on the minimum turning radius of the agricultural machinery, the lateral spacing of the harvesting rows, and the effective width of the field edge. The harvesting section along the working row is divided into straight segments within the working row. Then, based on the starting and ending positions of the turn calculated by the bow-shaped turn geometric model, the transition travel section from the end of the current working row to the starting point of the arc turn is divided into bow-shaped turn straight segments, the curved section of the arc turn is divided into bow-shaped turn arc segments, and the transition travel section from the end of the arc turn to the starting point of the next working row is also divided into bow-shaped turn straight segments. Thus, the global harvesting trajectory is decomposed into a continuous segmented structure of "straight segments within the working row → bow-shaped turn straight segments → bow-shaped turn arc segments → bow-shaped turn straight segments → straight segments within the next working row". The starting and ending positions of each trajectory segment are accurately calculated by the bow-shaped turn geometric model in combination with the working row position, ensuring that each trajectory segment is fully adapted to the movement constraints of the agricultural machinery and the field space.

[0044] The horizontal spacing of the work rows is obtained by measuring the planting ridge spacing of the plot on-site or by retrieving the planting records; the effective width of the field head is obtained by measuring the reserved space at the plot boundary on-site or by retrieving the plot planning map.

[0045] It is worth noting that in determining the straight sections within the working rows of agricultural machinery, the straight-line driving acceleration performance parameters and straight-line driving deceleration performance parameters represent the acceleration and deceleration related parameters possessed by the agricultural machinery itself, respectively; the target deceleration speed at the end of the row is the entry speed constraint of the subsequent trajectory segment: if the subsequent trajectory segment is not the last working row, it needs to be connected to a turning arc segment, which travels at 1 / 2 of the maximum speed to ensure the speed continuity between the exit of the deceleration segment at the end of the row and the entrance of the turning segment; if the subsequent trajectory segment is the last working row, the agricultural machinery stops after completion, there is no subsequent movement segment, and the target deceleration speed at the end of the row is 0.

[0046] In the determination of the straight segment of the bow-shaped U-turn, the acceleration at the starting boundary and the acceleration at the ending boundary are both defined as 0 to ensure a smooth transition in motion between the current trajectory segment and the trajectory segments connected before and after. The tangential velocity at the end of the arc segment is the uniform speed of the corresponding arc segment, which is set as half of the maximum speed under the turning condition of the agricultural machinery. The initial speed of the next line is the exit speed constraint of the previous trajectory segment: the first working line starts from a standstill and there is no turning segment in the previous line, so the initial speed of the next line is set to 0. The middle working line enters from the previous arc segment, so half of the maximum speed is set to ensure the speed continuity between the exit of the arc segment and the starting point of the acceleration segment.

[0047] In addition, the maximum speed of agricultural machinery is obtained from its design manual or factory parameters or preset through the control terminal based on actual harvesting requirements.

[0048] In one specific embodiment, in step S414, the starting point and ending point are defined according to the starting point of the arc segment of each U-turn and the starting point of the straight segment of the U-turn. The arc length is calculated based on the arc radius and the central angle radian, and the travel time is calculated in combination with the travel speed of the arc segment at the beginning of the road. The starting point boundary velocity and the ending point boundary velocity are defined according to the tangential direction of the starting point and the ending point, and the starting point boundary acceleration and the ending point boundary acceleration are both defined as 0. Specifically, this includes: Step S4141: Define the starting point and ending point of each arc segment of the U-turn and the starting point of the corresponding straight segment of the U-turn, respectively. Step S4142: Based on the radius of the arc and central angle radians Calculate the length of the arc : The travel time was calculated by combining the travel speed on the roundabout section at the edge of the field. : Step S4143: Define the starting point boundary velocity and the ending point boundary velocity according to the tangential direction of the starting point and the ending point, respectively, and define the starting point boundary acceleration and the ending point boundary acceleration as 0.

[0049] Specifically, based on the tangential direction of the starting and ending points of each arc segment of the bow-shaped U-turn, the travel speed of the arc segment is decomposed into velocity components in the corresponding directions, defining the starting boundary velocity and the ending boundary velocity. At the same time, the boundary acceleration is set to 0 to ensure smooth connection between the arc segment and the preceding and following straight segments in terms of velocity and acceleration, satisfying the kinematic constraints of agricultural machinery.

[0050] Thus, on the one hand, by precisely matching the tangential direction of the arc segment and the preceding and following trajectories to define the boundary velocity, the problem of abrupt trajectory connection when turning at the edge of the field is completely solved, avoiding jerking or sudden steering of the agricultural machinery; on the other hand, by setting the boundary acceleration to 0, combined with the smoothing constraint of the fifth-order polynomial, the agricultural machinery maintains a stable driving state during the arc turning process, reducing the risk of vehicle body swaying when turning; furthermore, based on the arc radius and central angle, the arc length and travel time are calculated, so that the arc segment trajectory completely fits the minimum turning radius constraint of the agricultural machinery, making full use of the field space to complete the row change, reducing the invalid travel distance, and further shortening the overall harvesting operation time in conjunction with the optimal harvesting sequence.

[0051] In one specific embodiment, in step S42, for each trajectory segment, respectively in direction and The direction establishes a fifth-order polynomial, specifically including: Step S421: In The direction establishes a fifth-order polynomial: In the formula: Represented as a time variable within the current trajectory segment; , , , , and All are represented as Polynomial coefficients in the direction; Step S422: In The direction establishes a fifth-order polynomial: In the formula: , , , , and All are represented as Polynomial coefficients in the direction.

[0052] In one specific embodiment, in step S43, based on the start position, end position, boundary velocity, boundary acceleration, and travel time of each trajectory segment, the following parameters are obtained: polynomial coefficients in the direction The formulas for calculating the polynomial coefficients in the direction include: Step S431: Let the start and end points of each trajectory segment be at... The position, boundary velocity, and boundary acceleration in the direction are respectively and The travel time is ,but: Step S432: Assume the start and end points of each trajectory segment are at... The position, boundary velocity, and boundary acceleration in the direction are respectively and ,but: In one specific embodiment, in step S44, based on the start and end points of the straight line segments within each work row, a normalized direction vector is obtained, and according to the corresponding boundary velocity and boundary acceleration, the end point of the acceleration segment and the start point of the deceleration segment are calculated. These are then substituted into the polynomial coefficient calculation formula to obtain the corresponding fifth-order polynomial expression, specifically including: Step S441: Let the starting point of each line segment within the work row be... The destination is Then the normalized direction vector for: Step S442: Let the distances of the acceleration segment, constant speed segment, and deceleration segment of each straight segment within the work row be respectively... , and Then the acceleration segment ends at the endpoint Deceleration phase start point and the end point They are respectively: Step S443: [The text appears to be incomplete and contains several grammatical errors. A more accurate translation would require the full context.] , and The starting point and ending point of the line segment within the work row corresponding to the fifth-order polynomial are defined respectively. Combined with the corresponding travel time, starting point boundary velocity, ending point boundary velocity, starting point boundary acceleration, and ending point boundary acceleration, the polynomial coefficients are substituted into the polynomial coefficient calculation formula to obtain the corresponding polynomial coefficients, thereby generating the corresponding fifth-order polynomial expression.

[0053] Specifically, by subdividing each straight segment within a work row into acceleration, uniform speed, and deceleration segments and fitting fifth-order polynomials to each segment, the actual operational dynamics of agricultural machinery entering the row, accelerating, harvesting at a uniform speed, and decelerating at the end of the row are accurately matched. This ensures that the speed and acceleration of the trajectory within the work row are continuous and smooth, thereby avoiding the problem of sudden speed changes caused by single fitting, and thus improving the stability and harvesting quality of straight-line operations of agricultural machinery.

[0054] In one specific embodiment, in step S45, the starting point, ending point, travel time, boundary velocity, and boundary acceleration corresponding to each arc-shaped U-turn straight segment are substituted into the polynomial coefficient calculation formula to obtain the corresponding fifth-order polynomial expression, specifically including: Extract the starting point, ending point, travel time, starting point boundary speed, and ending point boundary speed corresponding to each arc-shaped U-turn straight segment, and substitute them into the polynomial coefficient calculation formula to obtain the corresponding polynomial coefficients, thereby generating the corresponding fifth-order polynomial expression.

[0055] Specifically, by directly substituting the boundary parameters of each bow-shaped U-turn straight segment into the polynomial coefficient calculation formula, a smooth transition trajectory that meets the requirements for the connection of field headway speed can be quickly generated, ensuring that the agricultural machinery's driving state is continuous and without sudden changes from the bow-shaped U-turn arc segment to the straight segment of the next working row, thereby improving the stability and efficiency of field headway switching.

[0056] In one specific embodiment, in step S46, the starting point, ending point, travel time, boundary velocity, and boundary acceleration corresponding to each arc segment of the U-turn are substituted into the polynomial coefficient calculation formula to obtain the corresponding fifth-order polynomial expression, specifically including: Extract the starting point, ending point, travel time, starting point boundary velocity, and ending point boundary velocity corresponding to each arc segment of the U-turn, and substitute them into the polynomial coefficient calculation formula to obtain the corresponding polynomial coefficients, thereby generating the corresponding fifth-order polynomial expression.

[0057] Specifically, by directly substituting the starting point, ending point, travel time, starting point boundary speed, and ending point boundary speed of the arc segment of the bow-shaped turn into the polynomial coefficient calculation formula, a smooth arc trajectory that conforms to the turning radius constraint of the agricultural machinery can be quickly generated. This ensures that the speed and acceleration of the agricultural machinery are continuous and without sudden changes during the turn, achieving seamless connection between the turn at the edge of the field and the straight sections before and after, and improving the stability and efficiency of the agricultural machinery turning operation.

[0058] In one specific embodiment, in step S47: based on the optimal work row harvesting sequence, the straight segments within each work row, the straight segments of each bow-shaped turn, and the circular arc segments of each bow-shaped turn are sequentially spliced ​​together to generate a global reference trajectory, specifically including: Let the first The fifth-order polynomial expression for the trajectory segment is: Then the global reference trajectory for: In the formula: Indicates the total number of trajectory segments; And generate cumulative time series : Specifically, by splicing the fifth-order polynomial of each trajectory segment based on the optimal harvesting sequence of the work row and generating a cumulative time series, the position, velocity, and acceleration of the global reference trajectory can be continuously and smoothly maintained. This also provides a precise time index for subsequent trajectory tracking control, facilitating the rapid location of the current trajectory segment and the calculation of reference motion parameters, thereby improving the stability and efficiency of agricultural machinery automatic driving trajectory tracking.

[0059] In one embodiment, the path tracking control step S5 specifically includes: Step S51: Preset the coordinates of the front wheel center of the agricultural machinery in the world coordinate system, and construct the kinematic model of the rear-wheel steering two-wheel vehicle by combining its front and rear wheelbase, rear wheel angle, heading angle, driving speed and turning radius; Step S52: Based on the length of the line connecting the origin and the aiming point of the agricultural machinery in its on-board coordinate system, define its turning radius expression and calculate the expected rear wheel turning angle; Step S53: Based on the velocity adaptive pure tracking algorithm, define the dynamic adjustment expression for the pre-aiming distance of each trajectory segment; Step S54: Based on the current position of the agricultural machinery, extract the fifth-order polynomial expression of the current trajectory segment, and combine it with the corresponding dynamic adjustment aiming distance expression to construct the forward aiming position along the current heading of the agricultural machinery; Step S55: Extract the closest point to the current position of the agricultural machinery on the global reference trajectory, select the intersection point between it and the search line corresponding to the forward aiming position and define it as the target point, and calculate the current expected rear wheel angle of the agricultural machinery based on the target point; Step S56: Based on the global time corresponding to the nearest point on the global reference trajectory of the current position of the agricultural machinery, and combined with the preset forward look time, calculate the trajectory tangential speed at the predicted reference point and generate a longitudinal speed command. Input the longitudinal speed command and the current expected rear wheel angle into the chassis controller of the agricultural machinery to control the agricultural machinery to track and harvest along the global reference trajectory.

[0060] In one specific embodiment, in step S51, the coordinates of the front wheel center of the agricultural machinery in the world coordinate system are preset, and a kinematic model of the rear-wheel steering two-wheeled vehicle is constructed by combining its front and rear wheelbase, rear wheel steering angle, heading angle, driving speed, and turning radius. Specifically, this includes: Step S511: Let the front and rear wheelbase of the agricultural machinery be... The rear wheel turning angle is Turning radius is ,get: Step S512: Let the coordinates of the center of the front wheel of the agricultural machinery in the world coordinate system be... The heading angle is The driving speed is Construct a kinematic model of a rear-wheel steering two-wheeled vehicle: In the formula: Represented as the velocity component along the X-axis in the world coordinate system; Represented as the velocity component along the Y-axis in the world coordinate system; It is expressed as the rate of change of heading angle.

[0061] Specifically, by constructing a kinematic model of a rear-wheel steering two-wheeled vehicle, the core parameters and motion states of the agricultural machinery, such as the front and rear wheelbase, heading angle, and driving speed, are accurately correlated. This provides a theoretical basis that fits the actual motion characteristics of the agricultural machinery for the subsequent speed-adaptive pure tracking algorithm to calculate the expected rear wheel turning angle, thereby ensuring the accuracy and stability of path tracking control.

[0062] It is worth noting the front and rear wheelbase of agricultural machinery The fixed structural parameters of the agricultural machinery are obtained directly from its design manual or factory specifications; rear wheel steering angle. Real-time feedback is collected from sensors in the electro-hydraulic steering system of agricultural machinery; heading angle Attitude data acquired in real time from the GNSS-RTK navigation system mounted on the agricultural machinery; driving speed. The wheel speed signal is collected by the agricultural machinery wheel speed sensor and calculated by combining it with the wheel rolling radius.

[0063] In one specific embodiment, in step S52, based on the length of the line connecting the origin and the aiming point of the agricultural machinery in its onboard coordinate system, an expression for its turning radius is defined, and the desired rear wheel turning angle is calculated, specifically including: Step S521: Based on the origin and aiming point of the agricultural machinery in its onboard coordinate system The length of the line defines the aiming distance. To obtain the turning radius expression : In the formula: This is represented by the x-coordinate of the pre-aiming point of the agricultural machinery in its on-board coordinate system; Step S522: Calculate the desired rear wheel steering angle : In the formula: This refers to the front and rear wheelbase of the agricultural machinery. Specifically, the turning radius and the desired rear wheel steering angle are directly derived from the coordinates of the pre-aiming point in the vehicle coordinate system, which simplifies the geometric calculation logic of the subsequent speed adaptive pure tracking algorithm. This enables the rapid and accurate output of steering control commands, improving the response speed and control accuracy of agricultural machinery turning at the field and tracking curved trajectories.

[0064] In one specific embodiment, in step S53, based on the velocity adaptive pure tracking algorithm, a dynamic adjustment pre-aiming distance expression for each trajectory segment is defined, specifically including: Based on the velocity-adaptive pure tracking algorithm, a dynamic adjustment expression for the preview distance of each trajectory segment is defined. : In the formula: This is represented as the baseline value for pre-aiming distance; This represents the minimum aiming distance for a straight road segment; This represents the minimum aiming distance for a turning section; Expressed as speed on a straight section of road; This is expressed as the gain coefficient for the turning section; This represents the current speed of the agricultural machinery in the kth work row.

[0065] Specifically, by setting dynamic adjustment expressions for speed-related aiming distances for straight and turning road sections respectively, the core advantages of the existing speed-adaptive pure tracking algorithm can be leveraged. This allows agricultural machinery to maintain a sufficient aiming distance to ensure smooth driving as the driving speed increases during straight operations, while reducing the aiming distance and enhancing the response speed to curved paths by using a gain coefficient when turning at the edge of the field. This effectively balances the trajectory tracking accuracy and stability under different operating scenarios.

[0066] It is worth noting that the pre-aiming distance reference value in the embodiments of the present invention Minimum aiming distance on straight road sections Minimum aiming distance on turning sections Speed ​​on straight sections Gain coefficient for turning sections This allows agricultural machinery to maintain a larger aiming distance on straight operating sections, thereby improving driving smoothness; while a relatively smaller aiming distance is used on turning sections at the edge of the field to enhance the vehicle's responsiveness to curved paths.

[0067] In one specific embodiment, in step S54, based on the current position of the agricultural machinery, a fifth-order polynomial expression for the current trajectory segment is extracted, and combined with the corresponding dynamically adjusted aiming distance expression, a forward aiming position is constructed along the current heading of the agricultural machinery. Specifically, this includes: Step S541: Let the current position of the agricultural machinery be... Extract the fifth-order polynomial expression of the current trajectory segment. : In the formula: Represented as local time parameters within the current trajectory segment; This is represented as the current trajectory segment in The coefficients of the fifth-order polynomial in the direction, and ; This is represented as the current trajectory segment in The coefficients of the fifth-order polynomial in the direction; Step S542: Based on the corresponding dynamic adjustment aiming distance expression, construct the forward aiming position along the current heading of the agricultural machinery. : In the formula: This represents the actual aiming distance for the k-th work row.

[0068] Specifically, by combining the fifth-order polynomial expression of the current trajectory segment with the expression for dynamically adjusting the aiming distance, the aiming position is precisely constructed along the current heading of the agricultural machinery. This ensures the alignment between the aiming point and the current trajectory of the agricultural machinery, and allows for dynamic adjustment of the aiming range according to the work scenario. This effectively improves the accuracy and stability of trajectory tracking during straight-line harvesting and turning at the edge of the field.

[0069] In one specific embodiment, in step S55, the nearest point to the current position of the agricultural machinery on the global reference trajectory is extracted, and the intersection point between this point and the search line corresponding to the forward aiming position is selected and defined as the target point. The current expected rear wheel angle of the agricultural machinery is calculated based on the target point, specifically including: Step S551: Extract the closest point to the agricultural machinery's current position on the global reference trajectory. Draw a search line along the agricultural machinery's current heading, starting from the closest point. Define the intersection of the search line and the trajectory segment containing the closest point as the target point. If the trajectory segment containing the nearest point is a straight line segment within the work row or a straight line segment of a U-turn, then the target point is selected on the corresponding straight line segment and its extension line. If the trajectory segment containing the nearest point is a curved U-turn arc segment, then select the target point within the corresponding arc segment; If the target point is not found in the trajectory segment containing the nearest point, then select from the next adjacent trajectory segment until the target point is obtained; Step S552: Based on the target point : In the formula: and These are respectively represented as target points. The horizontal and vertical coordinates in the vehicle coordinate system; This is represented by the trajectory segment number where the target point is located; This is represented as the local time parameter of the target point within the corresponding trajectory segment; Calculating agricultural machinery in the first Current expected rear wheel angle in the work line : Specifically, starting from the nearest point on the global reference trajectory where the agricultural machinery is currently located, a search line is drawn along the current heading. If the trajectory segment where the nearest point is located is a straight segment within the work row or a straight segment of an arc-shaped turn, then a target point is selected on the corresponding straight segment and its extension line to prevent the agricultural machinery from turning prematurely when it approaches the end of the field, thus disrupting the harvesting rhythm. If the trajectory segment where the nearest point is located is an arc segment of an arc-shaped turn, then a target point is selected within the corresponding arc segment to ensure that a suitable target point can be quickly found when turning at the end of the field, thus ensuring the stability and accuracy of tracking control.

[0070] This differentiated search strategy avoids control disruptions caused by frequent jumps between target points on different trajectory segments. Furthermore, by calculating the current expected rear wheel angle, the agricultural machinery maintains smooth driving during straight-line harvesting and enhances trajectory tracking response speed when turning at the edge of the field, thereby effectively improving the accuracy and stability of trajectory tracking.

[0071] In one specific embodiment, in step S56, based on the global time corresponding to the nearest point on the global reference trajectory at the current position of the agricultural machinery, and combined with a preset forward look time, the trajectory tangential velocity at the predicted reference point is calculated and a longitudinal velocity command is generated. The longitudinal velocity command and the currently desired rear wheel steering angle are both input to the chassis controller of the agricultural machinery to control the agricultural machinery to track and harvest along the global reference trajectory. Specifically, this includes: Step S561: Let the global time corresponding to the nearest point on the global reference trajectory to the current position of the agricultural machinery be . Combined with preset forward look time Obtain the reference time for longitudinal velocity calculation. : Among them, if reference time If the total duration exceeds the global reference trajectory, it is determined that the agricultural machinery is approaching the end of the global reference trajectory, and the reference time is then used. The trajectory tangential velocity at the corresponding predicted reference point is 0; Otherwise, set a reference time. Located in the Within the trajectory segment, and the first The start time of the trajectory segment is Then the first Local time within the trajectory segment for: Step S562: For the first Taking the first derivative of the fifth-order polynomial expression of the trajectory segment, we obtain the predicted reference point at... direction and velocity components in the direction and The tangential velocity of the trajectory is obtained by synthesis. And generate longitudinal speed commands; Step S563: Input the longitudinal speed command and the current desired rear wheel angle into the chassis controller of the agricultural machine to control the agricultural machine to follow the global reference trajectory for harvesting.

[0072] Specifically, this embodiment uses the fifth-order polynomial expression of the global reference trajectory of agricultural machinery as its core basis, and achieves coordinated tracking control in the longitudinal and lateral directions by predicting reference points: First, based on the global time corresponding to the nearest point matched on the global reference trajectory according to the current position of the agricultural machinery, and combined with the preset forward look time, the reference time is calculated in advance to predict the speed change trend of the trajectory segment and avoid the agricultural machinery's driving speed from deviating from the trajectory speed abruptly. The preset forward look time in the embodiments of the present invention... .

[0073] It is defined that if the reference time exceeds the total duration of the global reference trajectory, then the agricultural machinery is determined to be close to the end of the global reference trajectory, and the reference time is then... The trajectory tangential velocity at the corresponding predicted reference point is 0 to ensure that the agricultural machinery stops smoothly at the harvesting end without any additional safety hazards.

[0074] By converting the global reference time into the local time of the corresponding trajectory segment, and then calling the polynomial parameters of the corresponding segment through the local time, the first derivative of the fifth-order polynomial expression is performed to obtain the trajectory tangential velocity and generate the longitudinal velocity command. Since the core of longitudinal velocity control is to make the agricultural machinery travel along the tangential direction of the reference trajectory, and the trajectory tangential velocity is the forward speed of the reference trajectory itself at the predicted reference point, using it as the longitudinal velocity command can ensure that the agricultural machinery's travel speed is accurately matched with the dynamic changes of the corresponding trajectory segment, achieving smooth tracking along the corresponding trajectory segment.

[0075] Furthermore, by inputting the longitudinal speed command and the current desired rear wheel steering angle into the chassis controller, coordinated control of longitudinal travel and lateral steering is achieved. This allows the agricultural machinery's travel speed to adapt to the dynamic changes in the trajectory segment in advance, avoiding vehicle vibration or a decrease in harvesting quality caused by sudden speed changes. At the same time, combined with lateral steering control, it not only ensures the smoothness of harvesting operations but also improves the tracking accuracy and safety of the agricultural machinery's global reference trajectory.

[0076] This invention uses an asymmetric trapezoidal cotton field model to construct a simulated plot. The simulated plot consists of several parallel work rows, used to simulate agricultural machinery harvesting operations under irregular field boundaries. The simulation parameters are set as follows: Table 1 Figure 2 This is a schematic diagram of a simulated plot of land according to an embodiment of the present invention. In the diagram, the horizontal axis is the X-axis, with units of meters, and the vertical axis is the Y-axis, with units of meters. The area between two adjacent blue lines represents a single work row.

[0077] To verify the effectiveness of the Held-Karp dynamic programming algorithm proposed in this invention for optimizing the row sequence of agricultural machinery operations, this invention compares and analyzes iterative greedy algorithm, nested row method, simulated annealing algorithm, and Held-Karp dynamic programming algorithm. All algorithms are planned under the same plot parameters, agricultural machinery parameters, and global operation time cost model, and the total operation time is calculated under conditions of 10, 15, and 20 operation rows, respectively.

[0078] The total job time of the above algorithms is compared as follows: Table 2 As shown in Table 2, the total operation time of all algorithms increases with the number of operation rows. In contrast, the Held-Karp dynamic programming algorithm achieves the optimal total operation time for all different numbers of operation rows, indicating that it can search for possible harvesting combinations of operation rows under a given global operation time cost model and directional alternation constraints. This avoids deviations caused by calculating turning or in-row time separately, thus solving the problems of heuristic algorithms easily getting trapped in local optima and the poor flexibility of adapting regular paths to irregular plots.

[0079] While one or more embodiments of the present invention have been described above, those skilled in the art will recognize that the present invention can be implemented in any other form without departing from its spirit and scope. Therefore, the embodiments described above are illustrative and not restrictive, and many modifications and substitutions will be apparent to those skilled in the art without departing from the spirit and scope of the invention as defined in the appended claims.

Claims

1. An optimal path planning and tracking control method for automatic agricultural machinery driving, characterized in that, include: The operation row map construction process involves obtaining the prior location information of the sowing rows in the plot to be harvested, mapping it to a unified coordinate system through coordinate transformation, and combining it with the operation width of agricultural machinery to convert adjacent rows into single benchmark operation rows. Several operation rows are then extended in parallel based on the single benchmark operation rows to construct the operation row map. Global operation time cost modeling step: Based on the distance between two adjacent operation rows and the number of cross-row intervals of the agricultural machinery, the field turning mode is determined and field turning time cost functions containing the turning segment movement speed and acceleration and deceleration parameters are constructed respectively. Intra-row operation time cost functions containing the entry acceleration, constant speed and end deceleration processes are generated to form a global operation time cost model. The optimal job row sequence output stage: Set the alternation constraint of the job direction of two adjacent job rows, and based on the preset dynamic programming algorithm, combined with the global job time cost model, obtain the optimal job row harvesting sequence that minimizes the global cumulative job time; Global reference trajectory generation: Based on the optimal work row harvesting sequence, the global harvesting trajectory is divided into several straight segments within the work row, several straight segments with bow-shaped turns, and several circular arc segments with bow-shaped turns. The starting point, ending point, and travel time of each trajectory segment are constrained, as well as the boundary velocity and boundary acceleration corresponding to the starting point and ending point. Each trajectory segment is fitted according to a fifth-order polynomial to generate the global reference trajectory. Path tracking control: Construct a kinematic model of a rear-wheel steering two-wheeled vehicle, and combine it with a speed adaptive pure tracking algorithm to define a dynamic adjustment expression for the pre-aiming distance of each trajectory segment. Based on the fifth-order polynomial expression corresponding to each trajectory segment, calculate the trajectory tangential velocity and generate a longitudinal velocity specification by combining the preset forward look time. Output the longitudinal velocity specification and the pre-acquired current expected rear wheel angle to the control terminal of the agricultural machine to control the agricultural machine to track and harvest along the global reference trajectory.

2. The optimal path planning and tracking control method for automatic agricultural machinery operation according to claim 1, characterized in that, The global job time cost modeling step specifically includes: Calculate the lateral crossing distance between two adjacent work rows based on the spacing between two adjacent work rows and the number of cross-row intervals of the agricultural machinery; The turning pattern is determined to be either fishtail or bow shape based on the lateral change distance and the minimum turning radius of the agricultural machinery; The fishtail turning pattern is divided into a circular arc forward deceleration segment, a reverse straight segment, and a circular arc forward acceleration segment. The time cost function of the fishtail turning at the end of the road is calculated. The bow-shaped turning pattern is divided into a circular arc constant speed segment and an oblique straight segment. The time cost function of the bow-shaped turning at the end of the road is calculated. Calculate the travel distance and entry distance of the agricultural machinery in the fishtail turn mode and bow turn mode, obtain the equivalent length of each work row, and generate the in-row operation time cost function by combining the corresponding acceleration distance and deceleration distance. By combining the time cost functions for fishtail-shaped turning points, bow-shaped turning points, and in-row operation time cost functions, a global operation time cost model is formed.

3. The optimal path planning and tracking control method for automatic agricultural machinery operation according to claim 2, characterized in that, The process of dividing the fishtail-shaped turning pattern into an arc-shaped forward deceleration segment, a reverse straight segment, and an arc-shaped forward acceleration segment, and calculating the time cost function for the fishtail-shaped turn, and dividing the bow-shaped turning pattern into an arc-shaped constant speed segment and an oblique straight segment, and calculating the time cost function for the bow-shaped turn, specifically includes: Based on the trajectory geometry of the fishtail turning pattern and the turning speed variation law of agricultural machinery, the fishtail turning pattern is divided into a circular arc forward deceleration section, a reverse straight section, and a circular arc forward acceleration section. Based on the division of the forward deceleration section, the reverse straight section, and the forward acceleration section of the circular arc, combined with the lateral lane change distance... Calculate the time cost function for turning at the fishtail-shaped headland. : In the formula: It is represented as the angle between the land parcel boundary and the direction of the work line; This represents the speed of travel on the circular section at the edge of the field. Expressed as the speed of travel on a straight segment, and ; Expressed as acceleration on a straight line segment; Expressed as deceleration on a straight line segment; It is represented as the acceleration of a fishtail-shaped circular arc segment, and ,in, Represented as The corresponding radian value; It is represented as the deceleration of a fishtail-shaped circular arc segment, and ; Based on the trajectory geometry of the bow-shaped turning pattern and the turning speed variation law of agricultural machinery, the bow-shaped turning pattern is divided into a circular arc uniform speed segment and an oblique straight line segment. Based on the division of the circular arc uniform speed segment and the oblique straight line segment, combined with the lateral line break distance Calculate the turning time cost function of the bow-shaped headland. : In the formula: This represents the minimum turning radius of the agricultural machinery.

4. The optimal path planning and tracking control method for automatic agricultural machinery operation according to claim 3, characterized in that, The global operation time cost model is formed by combining the fishtail-shaped turning time cost function, the bow-shaped turning time cost function, and the in-row operation time cost function, specifically including: Combining the fishtail-shaped turning time cost function Bow-shaped turning time cost function and in-row operation time cost function To form a global operation time cost model : In the formula: Represented as the first One work line; This is expressed as the headway turning time cost function between two adjacent work lines, and .

5. The optimal path planning and tracking control method for automatic agricultural machinery operation according to claim 4, characterized in that, The optimal job row order output process specifically includes: Define each work line as either a forward work state or a reverse work state, and set an alternating work direction constraint for two adjacent work lines; Based on the Held-Karp dynamic programming algorithm, combined with the operation state function within the harvesting plot, the additional time cost function between two adjacent operation rows is calculated, and the dynamic programming recursive equation is obtained. Each work row is defined as an independent starting candidate. The cumulative time of the starting state is the working time of the corresponding work row in the forward row. The harvested work rows are traversed according to the dynamic programming recursive equation, and the state is updated iteratively based on the number of them. In each round, one work row is selected from the unharvested work rows and the new transfer cost is calculated to update the cumulative time. When all work rows are harvested, the harvesting sequence corresponding to the termination work row with the smallest cumulative time is selected as the optimal work row harvesting sequence.

6. The optimal path planning and tracking control method for automatic agricultural machinery operation according to claim 5, characterized in that, Each work row is defined as an independent starting candidate. The cumulative time of the starting state is the in-row work time of the corresponding work row. The harvested work rows are traversed according to the dynamic programming recursive equation, and the state is updated iteratively based on their quantity. In each round, one work row is selected from the unharvested work rows and the new transfer cost is calculated to update the cumulative time. When all work rows have been harvested, the harvesting sequence corresponding to the terminated work row with the shortest cumulative time is selected as the optimal work row harvesting sequence, specifically including: Define each job line as an independent starting candidate, and set each starting job line as... Then its initial state cumulative time : In the formula: Indicates the initial working direction; This represents the in-row operation time of the starting job row; The harvested work rows are traversed according to the dynamic programming recursive equation, and the status is updated iteratively based on their quantity. In each round, one work row is selected from the unharvested work rows and added, and the new transfer cost is calculated and the cumulative time is updated. When all work lines have been collected, in all candidate termination work lines The work line with the minimum cumulative working time is selected as the termination work line, and the optimal work line harvesting sequence is obtained based on the work line harvesting sequence corresponding to the termination work line: In the formula: This represents the total number of rows in the operation. This represents the operational state function within the harvesting area.

7. The optimal path planning and tracking control method for automatic agricultural machinery operation according to claim 6, characterized in that, The global reference trajectory generation process specifically includes: Based on the optimal working row harvesting sequence and combined with the bow-shaped turn geometric model, the global harvesting trajectory is divided into several straight segments within the working row, several straight segments of bow-shaped turns, and several circular arc segments of bow-shaped turns. The starting point, ending point, and travel time of each trajectory segment are determined, as well as the boundary velocity and boundary acceleration corresponding to the starting point and ending point. For each trajectory segment, respectively in direction and The direction establishes a fifth-order polynomial: Based on the start and end positions, boundary velocities, boundary accelerations, and travel time of each trajectory segment, obtain... polynomial coefficients in the direction Formula for calculating polynomial coefficients in a direction; Based on the start and end points of the straight line segments in each work row, the normalized direction vector is obtained, and the end point of the acceleration segment and the start point of the deceleration segment are calculated according to the corresponding boundary velocity and boundary acceleration. These are then substituted into the polynomial coefficient calculation formula to obtain the corresponding fifth-order polynomial expression. Substituting the starting point, ending point, travel time, boundary velocity, and boundary acceleration corresponding to each bow-shaped U-turn straight segment into the polynomial coefficient calculation formula, we obtain the corresponding fifth-order polynomial expression. Substituting the starting point, ending point, travel time, boundary velocity, and boundary acceleration corresponding to each arc segment of the U-turn into the polynomial coefficient calculation formula, we obtain the corresponding fifth-order polynomial expression. Based on the optimal work row harvesting sequence, the straight segments within each work row, the straight segments of each bow-shaped turn, and the circular arc segments of each bow-shaped turn are sequentially spliced ​​together to generate a global reference trajectory.

8. The optimal path planning and tracking control method for automatic agricultural machinery operation according to claim 7, characterized in that, The optimal work row harvesting sequence involves sequentially splicing together straight segments within each work row, straight segments of each bow-shaped turn, and circular segments of each bow-shaped turn to generate a global reference trajectory. Specifically, this includes: Let the first The fifth-order polynomial expression for the trajectory segment is: Then the global reference trajectory for: In the formula: Indicates the total number of trajectory segments; And generate cumulative time series : 。 9. The optimal path planning and tracking control method for automatic agricultural machinery operation according to claim 8, characterized in that, The path tracking control mechanism specifically includes: The kinematic model of a rear-wheel steering two-wheeled vehicle is constructed by pre-setting the coordinates of the center of the front wheel of the agricultural machinery in the world coordinate system, and combining its front and rear wheelbase, rear wheel angle, heading angle, driving speed and turning radius. Based on the length of the line connecting the origin and the aiming point of the agricultural machinery in its on-board coordinate system, the expression for its turning radius is defined, and the expected rear wheel turning angle is calculated. Based on the speed-adaptive pure tracking algorithm, a dynamic adjustment expression for the pre-aiming distance of each trajectory segment is defined; Based on the current position of the agricultural machinery, extract the fifth-order polynomial expression of the current trajectory segment, and combine it with the corresponding dynamic adjustment of the aiming distance expression to construct the forward aiming position along the current heading of the agricultural machinery; Extract the closest point on the global reference trajectory to the current position of the agricultural machinery, select the intersection point between it and the search line corresponding to the forward aiming position and define it as the target point, and calculate the current expected rear wheel angle of the agricultural machinery based on the target point; Based on the global time corresponding to the nearest point on the global reference trajectory where the current position of the agricultural machinery is located, and combined with the preset forward look time, the trajectory tangential velocity at the predicted reference point is calculated and a longitudinal velocity command is generated. The longitudinal velocity command and the current expected rear wheel rotation angle are both input into the chassis controller of the agricultural machinery to control the agricultural machinery to track and harvest along the global reference trajectory.

10. The optimal path planning and tracking control method for automatic agricultural machinery operation according to claim 9, characterized in that, The method involves calculating the trajectory tangential velocity at the predicted reference point based on the global time corresponding to the nearest point of the agricultural machinery's current position on the global reference trajectory, combined with a preset forward look time, and generating a longitudinal velocity command. The longitudinal velocity command and the currently desired rear wheel steering angle are then input into the agricultural machinery's chassis controller to control the agricultural machinery to track and harvest along the global reference trajectory. Specifically, this includes: Let the global time corresponding to the nearest point on the global reference trajectory to the current position of the agricultural machinery be . Combined with preset forward look time Obtain the reference time for longitudinal velocity calculation. : Among them, if reference time If the total duration exceeds the global reference trajectory, it is determined that the agricultural machinery is approaching the end of the global reference trajectory, and the reference time is then used. The trajectory tangential velocity at the corresponding predicted reference point is 0; Otherwise, set a reference time. Located in the Within the trajectory segment, and the first The start time of the trajectory segment is Then the first Local time within the trajectory segment for: For the first Taking the first derivative of the fifth-order polynomial expression of the trajectory segment, we obtain the predicted reference point at... direction and velocity components in the direction and The tangential velocity of the trajectory is obtained by synthesis. And generate longitudinal speed commands; The longitudinal speed command and the current desired rear wheel angle are both input into the chassis controller of the agricultural machine to control the agricultural machine to follow the global reference trajectory for harvesting.

Images