Path planning navigation system for unmanned agricultural machines

Abstract

This application relates to the field of path navigation, specifically disclosing a path planning and navigation system for unmanned agricultural machinery. It first discretizes the continuous path and velocity space to construct a spatiotemporal state grid containing position and velocity states. This grid itself embeds all feasible trajectories that satisfy vehicle dynamics constraints. When evaluating the merits of any small segment of the trajectory in the grid, a multi-objective cost function is established, incorporating time efficiency, control smoothness, and variable operation application errors caused by actuator delays. Finally, a dynamic programming algorithm searches for the path with the minimum cumulative cost on this state grid, proactively generating an optimal spatiotemporal trajectory. This trajectory inherently guarantees the smoothness of velocity changes from the outset, actively avoiding the decrease in operational accuracy caused by vehicle inertia and actuator delays, thus fundamentally resolving the contradiction between operational quality and efficiency.

Description

Technical Field

[0001] This application relates to the field of path navigation, and more specifically, to a path planning and navigation system for unmanned agricultural machinery. Background Technology

[0002] With the rapid development of precision agriculture technology, unmanned agricultural machinery has become a key piece of equipment for improving agricultural production efficiency and optimizing resource allocation. In precision agriculture practice, variable-rate operations, such as variable-rate fertilization or spraying based on prescription maps, are one of its core applications. Prescription maps divide farmland into different operational areas based on crop growth, soil nutrients, and other information, and specify a particular target application rate for each area. To accurately execute prescription map instructions, unmanned agricultural machinery not only needs to travel along a predetermined path but also must dynamically adjust its operational parameters. One common and effective method is to control the application rate per unit area by adjusting the travel speed. Therefore, constructing a path planning and navigation scheme that can generate a solution that considers both path geometry and dynamic speed changes is of paramount importance for ensuring the quality and efficiency of variable-rate operations and promoting the intelligent development of unmanned agricultural machinery.

[0003] However, existing unmanned agricultural machinery path planning and navigation schemes have significant shortcomings when handling variable-task operations. Traditional schemes typically employ a decoupled strategy of path first, speed second. Specifically, a fixed geometric path is first generated based on the farmland boundary; then, during the machine's movement, the control system queries a prescription map based on the current location and commands the vehicle to adjust to the target speed in real time. The fundamental problem with this method is that it ignores the physical limitations of agricultural machinery as heavy equipment and the dynamic characteristics of the operating system. On the one hand, agricultural machinery has significant inertia and cannot respond instantaneously to step-speed commands. Forcing such commands can cause severe jerking, affecting energy consumption and machine lifespan, and potentially damaging the soil due to wheel slippage. On the other hand, spraying and other operational actuators have response delays. When the vehicle speed changes drastically at the area boundary, the adjustment of the application flow rate often lags behind the speed change, resulting in over-application at the beginning of the deceleration zone and under-application at the beginning of the acceleration zone. This severely affects the accuracy of variable-task operations, contradicting the goals of precision agriculture.

[0004] Therefore, an optimized path planning and navigation system for unmanned agricultural machinery is desired. Summary of the Invention

[0005] To address the aforementioned technical problems, this application is proposed. Embodiments of this application provide a path planning and navigation system for unmanned agricultural machinery.

[0006] According to one aspect of this application, a path planning and navigation system for unmanned agricultural machinery is provided, comprising:

[0007] The reference path generation module is used to generate reference paths based on farmland boundaries and farm implement widths.

[0008] The velocity mapping module is used to map the velocity of each point in the reference path based on the prescription map to obtain a reference rate profile;

[0009] The state grid construction module is used to construct a spatiotemporal state grid based on vehicle parameters and a reference path to obtain the state grid.

[0010] The grid edge cost calculation module is used to perform multi-objective edge cost calculation on the state grid based on the reference rate profile to obtain the cost of each edge in the state graph.

[0011] The spatiotemporal trajectory search module is used to perform dynamic programming-based optimal spatiotemporal trajectory search on the state lattice diagram to obtain the optimal spatiotemporal trajectory.

[0012] Beneficial Effects: Compared with existing technologies, the path planning and navigation system for unmanned agricultural machinery provided in this application first discretizes the continuous path and velocity space to construct a spatiotemporal state grid containing position and velocity states. This grid itself embeds all feasible trajectories that satisfy vehicle dynamics constraints. When evaluating the merits of any small segment of the trajectory in the grid, a multi-objective cost function is established, incorporating time efficiency, control smoothness, and variable operation application errors caused by actuator delays. Finally, a dynamic programming algorithm searches for the path with the minimum cumulative cost on this state grid, proactively generating an optimal spatiotemporal trajectory. This trajectory inherently guarantees the smoothness of velocity changes from the outset, actively avoiding the decrease in operational accuracy caused by vehicle inertia and actuator delays, thereby fundamentally resolving the contradiction between operational quality and efficiency. Attached Figure Description

[0013] The above and other objects, features, and advantages of this application will become more apparent from the more detailed description of the embodiments of this application in conjunction with the accompanying drawings. The drawings are provided to further illustrate the embodiments of this application and form part of the specification. They are used together with the embodiments of this application to explain this application and do not constitute a limitation thereof. In the drawings, the same reference numerals generally represent the same components or steps.

[0014] Figure 1 This is a system block diagram of a path planning and navigation system for unmanned agricultural machinery according to an embodiment of this application.

[0015] Figure 2 This is a schematic diagram of data flow in a path planning and navigation system for unmanned agricultural machinery according to an embodiment of this application.

[0016] Figure 3This is a block diagram of a state grid construction module in a path planning and navigation system for unmanned agricultural machinery according to an embodiment of this application.

[0017] Figure 4 This is a block diagram of the grid edge cost calculation module in a path planning and navigation system for unmanned agricultural machinery according to an embodiment of this application. Detailed Implementation

[0018] Hereinafter, exemplary embodiments according to this application will be described in detail with reference to the accompanying drawings. Obviously, the described embodiments are merely some embodiments of this application, and not all embodiments of this application. It should be understood that this application is not limited to the exemplary embodiments described herein.

[0019] As indicated in this application and claims, unless the context clearly indicates otherwise, the words "a," "an," "an," and / or "the" are not specifically singular and may include plural forms. Generally speaking, the terms "comprising" and "including" only indicate the inclusion of explicitly identified steps and elements, which do not constitute an exclusive list, and the method or apparatus may also include other steps or elements.

[0020] While this application makes various references to certain modules of the systems according to embodiments of this application, any number of different modules can be used and run on user terminals and / or servers. The modules described are merely illustrative, and different aspects of the systems and methods may use different modules.

[0021] Flowcharts are used in this application to illustrate the operations performed by the system according to embodiments of this application. It should be understood that the preceding or following operations are not necessarily performed in exact order. Instead, various steps can be processed in reverse order or simultaneously as needed. Furthermore, other operations can be added to these processes, or one or more steps can be removed from them.

[0022] Hereinafter, exemplary embodiments according to this application will be described in detail with reference to the accompanying drawings. Obviously, the described embodiments are merely some embodiments of this application, and not all embodiments of this application. It should be understood that this application is not limited to the exemplary embodiments described herein.

[0023] To address the technical problem in existing unmanned agricultural machinery path planning where the decoupling of path and speed planning leads to low accuracy and difficulty in balancing efficiency and smoothness, this application proposes a path planning and navigation system for unmanned agricultural machinery. This system generates a geometric reference path and its corresponding reference rate profile based on farmland boundaries and prescription maps. Subsequently, a refined spatiotemporal state grid is constructed: spatial dimension sampling is performed along the reference path to form a graph hierarchy, and speed is discretized at each level according to vehicle performance constraints, thereby generating nodes representing different spatiotemporal states. The connections (edges) between these nodes are not arbitrarily established but are strictly selected based on the vehicle's dynamics model to ensure that every state transition (acceleration / deceleration) is within physical feasibility. More importantly, this application assigns a multi-objective comprehensive cost to each edge. This cost includes not only time and energy costs but also, crucially, the cost of application error. This cost is accurately predicted by establishing an actuator dynamic response model to determine the deviation between the actual application amount and the target rate under speed changes. Finally, by applying a dynamic programming algorithm to perform a global search on this cost-weighted state lattice, a path with the minimum cumulative cost from the starting point to the ending point is found, thus obtaining an optimal spatiotemporal trajectory. This trajectory inherently guarantees the smoothness of speed changes from the outset and proactively avoids operational errors caused by actuator delays, fundamentally resolving the contradiction between accuracy, efficiency, and smoothness.

[0024] The technical solution of this application proposes a path planning and navigation system for unmanned agricultural machinery. Figure 1 This is a system block diagram of a path planning and navigation system for unmanned agricultural machinery according to an embodiment of this application. Figure 2 This is a schematic diagram of the data flow in a path planning and navigation system for unmanned agricultural machinery according to an embodiment of this application. Figure 1 and Figure 2 As shown, a path planning and navigation system 100 for unmanned agricultural machinery according to an embodiment of this application includes: a reference path generation module 110, used to generate a reference path based on farmland boundaries and implement width; a reference path generation module 120, used to perform velocity mapping on each point in the reference path based on a prescription map to obtain a reference rate profile; a state grid construction module 130, used to construct a spatiotemporal state grid based on vehicle parameters and the reference path to obtain a state grid; a grid edge cost calculation module 140, used to perform multi-objective edge cost calculation on the state grid based on the reference rate profile to obtain the cost of each edge in the state grid; and a spatiotemporal trajectory search module 150, used to perform dynamic programming-based optimal spatiotemporal trajectory search on the state grid to obtain an optimal spatiotemporal trajectory.

[0025] In the aforementioned path planning and navigation system 100 for unmanned agricultural machinery, the reference path generation module 110 generates a reference path based on the farmland boundary and the width of the implements. It should be understood that since subsequent spatiotemporal trajectory optimization needs to be performed under defined geometric constraints, directly searching for the optimal trajectory containing velocity and time dimensions in a two-dimensional farmland space would lead to excessively high computational complexity and difficulty in convergence. Therefore, in the technical solution of this application, a reference path is generated based on the farmland boundary and the width of the implements, thereby providing a one-dimensional spatial reference for the subsequent construction of the spatiotemporal state grid. This decomposes the complex trajectory optimization problem into two stages: geometric path generation and velocity profile optimization along the path, greatly reducing the difficulty of solving the problem and laying the foundation for achieving real-time, efficient optimal spatiotemporal trajectory planning.

[0026] Specifically, in this embodiment, the reference path generation module is used to: calculate the field width based on the number of field operation circles and the width of the farm implements, wherein the field width is equal to the product of the number of field operation circles and the width of the farm implements; perform an inward offset operation on the farmland boundary to obtain the main operation area and the main operation direction angle, wherein the offset distance is the field width; generate parallel operation path segments in the main operation area based on the main operation direction angle and the width of the farm implements to obtain a list of discontinuous operation path segments; and smoothly connect the list of discontinuous operation path segments to obtain the reference path. In other words, the process of generating the reference path first divides the plot into regions to distinguish between straight operation areas and field turning areas, then generates a series of parallel reciprocating operation paths within the main operation area, and finally connects these discontinuous path segments into a complete continuous path covering the entire field through smooth turning paths.

[0027] More specifically, in a concrete example of this application, for a rectangular farmland with vertex coordinates (0,0), (100,0), (100,50), (0,50), the width of the farm implement is 10 meters, and the number of field work circles is 1. First, based on the number of field work circles 1 and the width of the farm implement 10 meters, the width of the field is calculated to be 10 meters. Then, an inward offset operation with an offset distance of 10 meters is performed on the boundary of the farmland to obtain a main work area with vertices (10,10), (90,10), (90,40), (10,40), and the direction of its longest side is determined to be the main work direction angle, i.e., 0 degrees. Next, based on the main operating direction angle of 0 degrees and the width of the farm implements of 10 meters, three discontinuous parallel operating path segments are generated within the main operating area: from (10,15) to (90,15), from (10,25) to (90,25), and from (10,35) to (90,35). Finally, the discontinuous operating path segments are smoothly connected by turning, connecting the end point (90,15) of the first segment with the end point (90,25) of the second segment through a field-end turning path, and then connecting the starting point (10,25) of the second segment with the starting point (10,35) of the third segment, ultimately forming a complete reference path point sequence that starts at (10,15), travels in an S-shaped back-and-forth pattern, and ends at (90,35).

[0028] In the aforementioned path planning and navigation system 100 for unmanned agricultural machinery, the reference path generation module 120 performs velocity mapping on each point in the reference path based on a prescription map to obtain a reference rate profile. It should be understood that since the reference path generated in the previous step only contains geometric location information and does not inherently carry the agronomic instructions required for variable operations, the subsequent trajectory optimization process lacks a clear operational objective. Therefore, in the technical solution of this application, velocity mapping is further performed on each point in the reference path based on the prescription map to obtain a reference rate profile, thereby precisely binding the path points in the spatial dimension with the target application rate required by agronomy. It is worth mentioning that the prescription map here is a polygonal map, and each region of the polygonal map has a target application rate, which can generate a reference rate profile as an optimization benchmark, providing the necessary data input for subsequent calculation of operational application error costs and ultimately finding the optimal spatiotemporal trajectory that can accurately execute the variable operation task.

[0029] Specifically, in this embodiment, the velocity mapping module is used to: traverse each point on the reference path, and obtain the target application rate corresponding to each point by querying the prescription map to obtain the reference velocity profile. That is, the generation process of this velocity profile involves traversing all discrete points on the reference path, querying the prescription map by spatial location, and assigning a target application rate to the region where each path point is located.

[0030] More specifically, in a specific example of this application, the prescription map divides the farmland into two polygonal regions: the region with X coordinates ranging from 0 to 50 meters is the low application zone, with a target application rate of 10 L / ha; the region with X coordinates ranging from 50 to 100 meters is the high application zone, with a target application rate of 20 L / ha. When the system processes the aforementioned generated reference path, it traverses every coordinate point on the path. For the first segment of the path, i.e., the segment from (10,15) to (90,15), the system queries and finds that the parts with X values ​​less than 50 meters fall within the low application zone, so it assigns them a target application rate of 10 L / ha; while the parts with X values ​​greater than or equal to 50 meters fall within the high application zone, and it assigns them a target application rate of 20 L / ha. The same query operation is performed on subsequent turning paths and reciprocating operation path segments, ultimately obtaining a complete reference rate profile consisting of a series of 10 L / ha and 20 L / ha values ​​that corresponds one-to-one with the reference path point sequence.

[0031] In the aforementioned path planning and navigation system 100 for unmanned agricultural machinery, the state graph construction module 130 is used to construct a spatiotemporal state graph based on vehicle parameters and a reference path to obtain the state graph. It should be understood that since the movement of unmanned agricultural machinery in the physical world is a continuous process, directly searching for the optimal trajectory that balances multiple optimization objectives in a continuous spatiotemporal domain is computationally infeasible and cannot guarantee finding a globally optimal solution. Therefore, in the technical solution of this application, a spatiotemporal state graph is further constructed based on vehicle parameters and a reference path to obtain the state graph. This discretizes the infinite possibilities of continuous trajectories into a finite networked search space composed of state nodes and feasible transition edges. In this way, the complex continuous domain optimization problem can be transformed into a discrete shortest path problem that can be solved on a graph, providing a structured foundation for subsequently using efficient algorithms such as dynamic programming to find the globally optimal spatiotemporal trajectory.

[0032] Figure 3 This is a block diagram of a state grid construction module in a path planning and navigation system for unmanned agricultural machinery according to an embodiment of this application. Figure 3 As shown in the embodiments of this application, the state graph construction module 130 includes: a spatial dimension sampling unit 131, used to perform spatial dimension sampling on the reference path along the reference path to obtain a hierarchical sampling point list; a state dimension sampling unit 132, used to perform state dimension sampling on each hierarchical sampling point in the hierarchical sampling point list based on vehicle parameters to obtain a state graph node set; and a connection relationship construction unit 133, used to construct the connection relationship between state graph nodes in the state graph node set based on vehicle parameters to obtain a state graph edge set.

[0033] Accordingly, the state dimension sampling unit includes: first, extracting first-level sampling points from the hierarchical sampling point list; then, calculating the upper limit of velocity of the first-level sampling points using the following formula:

[0034]

[0035] Among them, v max For the maximum speed allowed at this position, a lat,max For the maximum permissible lateral acceleration, κ i v represents the curvature at that location, min represents the minimum value. max,1 The upper limit of the velocity for the first-level sampling points is set; finally, within the allowed velocity range of the first-level sampling points, a series of discrete velocity values ​​are generated with the velocity sampling resolution as the step size, and a state node is created for each discrete velocity value to obtain the first-level state node set.

[0036] Specifically, the spatiotemporal state lattice construction process involves discretizing the one-dimensional reference path in both spatial and state dimensions, and establishing connections between discrete states based on vehicle dynamics constraints. More specifically, in a concrete example of this application, the vehicle parameters are set to a maximum speed of 3 m / s and a maximum longitudinal acceleration of 1.0 m / s². 2 Maximum longitudinal deceleration -1.5 m / s² 2 First, spatial sampling is performed along the reference path, with a spatial step size of 1.0 meter. The continuous reference path is discretized into a series of hierarchical sampling points, each recording its arc length, coordinates, and local curvature. Second, state-dimensional sampling is performed. At each hierarchical sampling point, the upper limit of velocity at that location is calculated based on the curvature and the vehicle's maximum permissible lateral acceleration. Within this velocity range, a series of discrete velocity values ​​are generated with a velocity resolution of 0.5 m / s. Each position-velocity combination constitutes a state grid node. Finally, the connection relationships between nodes are constructed. For any two adjacent nodes in any two levels (e.g., level i and level i+1), the longitudinal acceleration required to transition from the velocity state of the former node to the velocity state of the latter node is calculated. For example, to transition from a node in level i with a velocity of 2.0 m / s to a node in level i+1 with a velocity of 2.5 m / s, the required acceleration is (2.5 m / s). 2 -2.0 2 ) / (2*1.0)=1.125m / s 2 Because this value exceeds 1.0 m / s 2 Due to the maximum acceleration limit, no connection edge is established between these two nodes. Conversely, if the required acceleration is in the range of [-1.5, 1.0] m / s², no connection edge is established between them. 2If the allowed range is reached, a directed edge is established between the two nodes. By performing this judgment on all adjacent node pairs, a complete state grid diagram containing all physically feasible trajectories is finally constructed.

[0037] In the aforementioned path planning and navigation system 100 for unmanned agricultural machinery, the grid edge cost calculation module 140 is used to perform multi-objective edge cost calculation on the state grid based on a reference rate profile to obtain the cost value of each edge in the state grid. It should be understood that since the state grid only describes all physically feasible trajectories, it does not provide a standard for evaluating the merits of these trajectories, especially how to balance conflicting performance indicators such as time efficiency, operational accuracy, and energy consumption within a unified framework. Therefore, in the technical solution of this application, multi-objective edge cost calculation is further performed on the state grid based on a reference rate profile to obtain the cost value of each edge in the state grid, thereby assigning a scalar cost that can accurately quantify the comprehensive performance of each edge in the graph (i.e., each tiny state transition). In this way, the complex multi-objective optimization problem can be transformed into a standard single-objective shortest path problem, enabling the subsequent dynamic programming algorithm to find a globally optimal spatiotemporal trajectory that comprehensively performs best across all performance dimensions.

[0038] Figure 4 This is a block diagram of the grid edge cost calculation module in a path planning and navigation system for unmanned agricultural machinery according to an embodiment of this application. Figure 4 As shown in the embodiments of this application, the grid edge cost calculation module 140 includes: a first edge extraction unit 141, used to extract a first edge from the state grid; a first edge cost calculation unit 142, used to calculate the time cost, operation application error cost, and energy consumption / control smoothness cost of the first edge; and a first edge cost calculation unit 143, used to determine the cost of the first edge based on the time cost, operation application error cost, and energy consumption / control smoothness cost of the first edge.

[0039] Accordingly, the first edge extraction unit 141 and the first edge cost calculation unit 142 are used to extract the first edge from the state lattice diagram and calculate the time cost, operation application error cost, and energy consumption / control smoothness cost of the first edge. It should be understood that in existing unmanned agricultural machinery variable operation path planning, the cost assessment of the trajectory generally adopts a linear weighted summation model. This model linearly combines multiple mutually constraining optimization objectives such as time efficiency, operation accuracy, and energy consumption smoothness through preset static weights to form a single cost scalar. This approach has inherent limitations. Specifically, firstly, the physical dimensions and numerical scales of each objective function differ greatly, making the weight setting highly sensitive and counterintuitive; even small weight changes can overturn the overall optimization direction, lacking engineering robustness. Secondly, this model treats each optimization objective as independent, essentially ignoring the complex nonlinear coupling relationship and the trade-off characteristics between them, failing to effectively explore the equilibrium solution located on the Pareto optimal boundary. Finally, static weights cannot adapt to the dynamically changing needs of operations. For example, the requirements for operational accuracy and smoothness should be significantly different when entering complex turning areas versus driving straight in open terrain. A one-size-fits-all cost assessment cannot achieve this kind of refined scenario adaptation. In other words, directly using linear weighted summation of raw costs with different physical dimensions and numerical scales, such as time, error, and energy consumption, makes weight setting extremely difficult and the results unstable. Even a small adjustment to the weights can overturn the optimization direction, and simple addition cannot reflect the decisive impact of extreme performance degradation in a certain dimension on the overall performance.

[0040] To address the aforementioned issues, a dynamic adaptive cost evaluation mechanism based on probabilistic utility fusion is proposed. This mechanism abandons direct linear weighting and instead models and fuses the ideality of each optimization objective within a probability space. Specifically, in this embodiment, the first edge value calculation unit is used to: perform probabilistic utility nonlinear mapping on time cost, operation application error cost, and energy consumption / control smoothness cost to obtain time probabilistic utility value, error probabilistic effect value, and energy consumption probabilistic utility value; perform joint utility calculation on the time probabilistic utility value, error probabilistic effect value, and energy consumption probabilistic utility value based on the conditional independence assumption to obtain a comprehensive utility value; and calculate the logarithmic function value of the comprehensive utility value as the value of the first edge. In this way, the bottleneck effect can be introduced through multiplicative fusion to ensure that any serious performance defect in any single dimension will lead to a sharp decrease in the comprehensive evaluation, and the non-additive utility index can be transformed into an additive value that can be directly used in the standard graph search algorithm through logarithmic transformation, thereby achieving robust and balanced multi-objective optimization.

[0041] More specifically, firstly, a probabilistic utility nonlinear mapping is performed on time cost, application error cost, and energy consumption / control smoothness cost to obtain probabilistic utility values ​​for time, error, and energy consumption. It should be understood that since these three cost indicators—time, application error, and energy consumption / control smoothness—have completely different physical dimensions and numerical scales, directly combining them would lead to the largest value dominating the optimization result, failing to achieve a balanced consideration of multiple objectives, and their inherent nonlinear constraints are difficult to express using a simple linear model. Therefore, in the technical solution of this application, a probabilistic utility nonlinear mapping is further performed on time cost, application error cost, and energy consumption / control smoothness cost to obtain probabilistic utility values ​​for time, error, and energy consumption, thereby uniformly mapping the original costs with different physical meanings to dimensionless utility values ​​in the [0,1] interval. This eliminates the differences in dimensions and scales among various cost items, making them uniformly comparable and interpretable, i.e., a probabilistic expression of the degree of ideality, laying the foundation for subsequent robust multi-objective fusion.

[0042] Specifically, regarding the cost of application error C in the operation. A (e) and energy consumption / control smoothness cost C E (e) has an ideal value of zero. A Gaussian radial basis function is used to map it to express the characteristic that utility decreases sharply with increasing cost. This can be expressed as:

[0043]

[0044] Where 'e' represents an edge in the state lattice diagram, 'X' represents the type of cost, and 'C' represents the value of the cost. X (e) represents the original cost value of e in the X dimension, σ X σ represents the sensitivity or scaling parameter of cost in dimension X, used to control the rate at which utility values ​​decay as cost increases. X The smaller the value, the more sensitive the utility function is to increases in cost and the faster it decays. exp represents the value of an exponential function with the natural constant e as its base. X (e) represents the probabilistic utility value of e in the X dimension.

[0045] Regarding time cost C T (e) Its optimization is not without a lower bound, and its marginal benefits diminish. An inverse sigmoid function is used for mapping to reflect its greater sensitivity to time costs around a certain threshold. This can be expressed as:

[0046]

[0047] Among them, C T(e) represents the original cost value of e in the time dimension, i.e., the time required to traverse this edge, k T The slope parameter, U, represents the time-cost utility function and controls the steepness of the function near the threshold t0. T (e) represents the probabilistic utility value of edge e in the time dimension, and t0 represents the threshold or center point of time cost, which defines the region where the utility function is most sensitive, when C T (e) equals t0, and the utility value is 0.5.

[0048] In this way, the differences in dimensions and scales among various cost items are successfully eliminated through nonlinear mapping. For error and energy consumption, the mapping reflects a high sensitivity to deviations from the ideal zero value; for time, it reflects a high penalty for extreme time consumption and a limited incentive for excessive time saving, which is more in line with the actual value judgment of agricultural operations.

[0049] Then, a joint utility calculation based on the conditional independence assumption is performed on the time probabilistic utility value, the error probabilistic effect value, and the energy consumption probabilistic utility value to obtain a comprehensive utility value. It should be understood that simple addition or linear weighted fusion cannot reflect the veto logic in multi-objective optimization; that is, extreme poor performance in a single dimension can be masked by excellent performance in other dimensions, potentially leading the path planner to incorrectly select trajectory segments with serious deficiencies in key performance aspects (such as operational accuracy). Therefore, in the technical solution of this application, a joint utility calculation based on the conditional independence assumption is further performed on the time probabilistic utility value, the error probabilistic effect value, and the energy consumption probabilistic utility value to obtain a comprehensive utility value, thereby fusing the independent utility values ​​into a joint utility index that reflects overall excellence.

[0050] Specifically, based on the assumption that each utility is conditionally independent on a given trajectory segment e, the combined utility is calculated by multiplying the individual utilities, and a joint utility function is constructed, expressed as:

[0051] U joint (e)=U T (e)·U A (e)·U E (e)

[0052] Among them, U joint (e) represents the overall utility value, U A (e) is the probabilistic utility value of e along dimension A, U E (e) is the probabilistic utility value of e in the E dimension.

[0053] It is understandable that the multiplicative fusion mechanism naturally introduces the weakest link effect. If any utility component approaches zero (indicating extremely poor performance in that dimension), the combined utility will rapidly approach zero. This mechanism ensures that the selection of trajectory segments must be a balanced result across all aspects, preventing the path planner from incorrectly adopting a segment due to extreme deterioration in a single metric.

[0054] Finally, the logarithmic function value of the combined utility is calculated as the cost of the first edge. It should be understood that since the combined utility value obtained in the previous step is obtained through multiplication, it is not additive. Mainstream efficient path search algorithms, such as Dijkstra's or A* algorithms, focus on finding the path with the minimum sum of accumulated costs, which requires that the weights or costs of the edges be additive. Therefore, in the technical solution of this application, the logarithmic function value of the combined utility is further calculated as the cost of the first edge, thereby losslessly converting the multiplicative joint utility index into an additive cost scalar. This transforms the complex optimization problem of maximizing the product of joint utility into an equivalent problem of minimizing the sum of accumulated costs, allowing seamless integration into mature graph search algorithm frameworks for finding globally optimal paths. This can be expressed as:

[0055] Cost log (e)=-log2(U joint (e))

[0056] Cost log (e) represents the cost of the first side.

[0057] It should be understandable that the converted Cost log (e) It has a clear information theory meaning and can be regarded as the degree of non-ideality or negative information content of the trajectory segment. The higher the utility, the closer the cost is to zero; the lower the utility, the more the cost increases sharply. This not only retains the advantages of multiplicative fusion, but also makes the cost additive, which can directly serve the path planning algorithm and finally search for a globally optimal trajectory with the minimum cumulative negative information content.

[0058] In the aforementioned path planning and navigation system 100 for unmanned agricultural machinery, the spatiotemporal trajectory search module 150 is used to perform dynamic programming-based optimal spatiotemporal trajectory search on the state grid to obtain the optimal spatiotemporal trajectory. It should be understood that since the previous steps have transformed the complex spatiotemporal trajectory optimization problem into a shortest path problem on a weighted state grid, but this graph contains a massive number of feasible paths, a systematic and efficient algorithm is needed to find the globally optimal solution. Therefore, in the technical solution of this application, a dynamic programming-based optimal spatiotemporal trajectory search is further performed on the state grid to obtain the optimal spatiotemporal trajectory, thereby efficiently solving the shortest path problem while ensuring global optimality. In this way, a path with the minimum cumulative cost can be deterministically found from the vast search space. The spatiotemporal trajectory represented by this path is the final execution scheme that achieves the best balance among multiple objectives such as time efficiency, operational accuracy, and control smoothness.

[0059] Specifically, in this embodiment, the optimal spatiotemporal trajectory search based on dynamic programming is performed on the state grid to obtain the optimal spatiotemporal trajectory, including: inputting the state grid into the A* algorithm module to obtain the optimal spatiotemporal trajectory. That is, the search process for the optimal spatiotemporal trajectory involves taking the weighted state grid as input, using the principle of dynamic programming, starting from the initial state, calculating and updating the optimal path and its cumulative cost to each state node layer by layer, until the endpoint is reached. More specifically, in a specific example of this application, the aforementioned constructed and edge cost calculated state grid is input into the A* algorithm module. The search starts from the initial state node representing the farmland entrance position (arc length of 0) and with a velocity of 0. In each iteration, the A* algorithm selects a node with the minimum comprehensive evaluation value (i.e., the actual cumulative cost from the starting point to the current node plus the estimated cost from the current node to the endpoint) from the set of nodes to be explored for expansion. For example, when the algorithm explores to the middle of the reference path, for the aforementioned edge whose cost value is as high as 2.19 due to the drastic change in job application rate, the cumulative cost value of any candidate path passing through this edge will increase significantly. Therefore, the A* algorithm will tend to expand other paths that bypass this high-cost edge, such as choosing a path that slows down the pace of work in advance, although the time cost is slightly higher, the cost of job application error is extremely low. This process continues, and the algorithm advances forward between different levels of the graph until the search reaches any terminal node representing the farmland exit position. At this point, the algorithm can reconstruct a sequence of nodes from the start point to the end point with the minimum cumulative cost value by backtracking the parent node pointers. This sequence is the final output optimal spatiotemporal trajectory.

[0060] In summary, the path planning and navigation system for unmanned agricultural machinery according to the embodiments of this application is explained. It first discretizes the continuous path and velocity space to construct a spatiotemporal state grid containing position and velocity states. This grid itself embeds all feasible trajectories that satisfy vehicle dynamics constraints. When evaluating the merits of any small segment of the trajectory in the grid, a multi-objective cost function is established, incorporating time efficiency, control smoothness, and variable operation application errors caused by actuator delays. Finally, a dynamic programming algorithm searches for the path with the minimum cumulative cost on the state grid, proactively generating an optimal spatiotemporal trajectory. This trajectory inherently guarantees the smoothness of velocity changes from the outset, actively avoiding the decrease in operational accuracy caused by vehicle inertia and actuator delays, thereby fundamentally resolving the contradiction between operational quality and efficiency.

[0061] The various embodiments of this disclosure have been described above. These descriptions are exemplary and not exhaustive, nor are they limited to the disclosed embodiments. Many modifications and variations will be apparent to those skilled in the art without departing from the scope and spirit of the described embodiments. The terminology used herein is chosen to best explain the principles, practical application, or improvement of the technology in the market, or to enable others skilled in the art to understand the embodiments disclosed herein.

Claims

1. A path planning and navigation system for unmanned agricultural machinery, characterized in that, include: The reference path generation module is used to generate reference paths based on farmland boundaries and farm implement widths. The velocity mapping module is used to map the velocity of each point in the reference path based on the prescription map to obtain a reference rate profile; The state grid construction module is used to construct a spatiotemporal state grid based on vehicle parameters and a reference path to obtain a state grid. The state grid construction process involves discretizing the one-dimensional reference path in both spatial and state dimensions, and establishing the connection relationship between discrete states based on vehicle dynamics constraints. The grid edge cost calculation module is used to perform multi-objective edge cost calculation on the state grid based on the reference rate profile to obtain the cost of each edge in the state graph. The spatiotemporal trajectory search module is used to perform dynamic programming-based optimal spatiotemporal trajectory search on the state lattice diagram to obtain the optimal spatiotemporal trajectory.

2. The path planning and navigation system for unmanned agricultural machines of claim 1, wherein, The reference path generation module is used for: The width of the field edge is calculated based on the number of field edge operation circles and the width of the farm implements. The width of the field edge is equal to the product of the number of field edge operation circles and the width of the farm implements. An inward offset operation is performed on the farmland boundary to obtain the main operating area and the main operating direction angle, where the offset distance is the width of the field edge; Based on the main operation direction angle and the width of the implement, parallel operation path segments are generated in the main operation area to obtain a list of discontinuous operation path segments; The list of discontinuous job path segments is smoothly connected by turning to obtain the reference path.

3. The path planning and navigation system for unmanned agricultural machines of claim 1, wherein, The prescription map is a polygon map, and each region of the polygon map has a target application rate.

4. The path planning and navigation system for unmanned agricultural machines of claim 3, wherein, The speed mapping module is used to: traverse each point on the reference path and obtain the target application rate corresponding to each point by querying the prescription map to obtain the reference speed profile.

5. The path planning and navigation system for unmanned agricultural machinery according to claim 1, characterized in that, The state grid construction module includes: A spatial dimension sampling unit is used to perform spatial dimension sampling along the reference path to obtain a hierarchical sampling point list. The state dimension sampling unit is used to perform state dimension sampling on each level sampling point in the list of level sampling points based on vehicle parameters to obtain a set of state grid nodes. The connection relationship construction unit is used to construct the connection relationships between state grid nodes in the state grid node set based on vehicle parameters to obtain the state grid edge set.

6. The path planning and navigation system for unmanned agricultural machines of claim 5, wherein, The state dimension sampling unit is used for: Extract the first-level sampling points from the list of hierarchical sampling points; The upper limit of velocity for the first level of sampling points is calculated using the following formula: ； in, The maximum speed allowed for the first level of sampling points. For the maximum permissible lateral acceleration, The curvature of the first level sampling points, To obtain the minimum value; Within the allowed velocity range of the first-level sampling points, a series of discrete velocity values ​​are generated with the velocity sampling resolution as the step size, and a state node is created for each discrete velocity value to obtain the first-level state node set.

7. The path planning and navigation system for unmanned agricultural machines of claim 1, wherein, The grid edge cost calculation module includes: The first edge extraction unit is used to extract the first edge from the state lattice diagram; The first side cost calculation unit is used to calculate the time cost, operation application error cost and energy consumption / control smoothness cost of the first side. The first-side cost calculation unit is used to determine the cost of the first side based on the time cost, operation application error cost, and energy consumption / control smoothness cost of the first side.

8. The path planning and navigation system for unmanned agricultural machines of claim 7, wherein, The first edge value calculation unit is used for: A probabilistic utility nonlinear mapping is performed on time cost, operational application error cost, and energy consumption / control smoothness cost to obtain time probabilistic utility value, error probabilistic effect value, and energy consumption probabilistic utility value; A combined utility value is obtained by performing joint utility calculations based on the conditional independence assumption on the time probabilistic utility value, the error probabilistic effect value, and the energy consumption probabilistic utility value. The logarithmic function value of the overall utility is calculated as the cost of the first side.

9. The path planning and navigation system for unmanned agricultural machinery according to claim 1, characterized in that, The spatiotemporal trajectory search module is used to input the state grid into the A algorithm module to obtain the optimal spatiotemporal trajectory.

Images