Agricultural machine track planning method and system

Abstract

The invention provides an agricultural machine track planning method and system, and the method comprises the steps: obtaining a preset global path, agricultural machine pose information, and obstacle information; determining a global path starting point according to a preset global path; the agricultural machine pose information comprises the current position, speed and orientation of the agricultural machine; in response to that the current position of the agricultural machine reaches the starting point of the global path, generating a reference trajectory along a preset global path through an interpolation method; performing collision detection on the reference trajectory according to the obstacle information; and in response to a collision detection result that the agricultural machine can collide with the obstacle, planning an agricultural machine obstacle avoidance bypassing track through an A * search algorithm based on dynamic constraint, the agricultural machine pose information and the obstacle information. The performance of the agricultural machine in the obstacle bypassing scene is improved, including the efficiency of planning the obstacle avoidance bypassing track of the agricultural machine and the smoothness of the obstacle bypassing track of the agricultural machine.

Description

Technical Field

[0001] This disclosure relates to the field of intelligent agricultural technology, and in particular to a method and system for planning agricultural machinery trajectories. Background Technology

[0002] Current agricultural scenarios require planning full-coverage paths for the work site. Existing local path generation solutions are mostly point-to-point path search algorithms, which lack adaptation to full-coverage paths. Especially when dealing with obstacle detour tasks, the algorithm efficiency is often constrained by the excessive number of searches, affecting real-time performance. Summary of the Invention

[0003] The technical problem to be solved by this disclosure is to overcome the shortcomings of low accuracy and low efficiency in the existing technology of global planning, and to provide a planning method and system for agricultural machinery trajectory.

[0004] This disclosure solves the above-mentioned technical problems through the following technical solution:

[0005] This disclosure relates to a method for planning the trajectory of agricultural machinery, the method comprising:

[0006] Obtain the preset global path, agricultural machinery pose information, and obstacle information;

[0007] The global path starting point is determined according to the preset global path; the agricultural machinery pose information includes the current position, speed, and orientation of the agricultural machinery;

[0008] In response to the fact that the current position of the agricultural machinery has reached the starting point of the global path, a reference trajectory is generated along the preset global path using an interpolation method;

[0009] Collision detection is performed on the reference trajectory based on the obstacle information;

[0010] In response to the collision detection result indicating that the agricultural machinery will collide with an obstacle, the obstacle avoidance trajectory of the agricultural machinery is planned by using the A* search algorithm based on dynamic constraints, the agricultural machinery pose information and obstacle information.

[0011] Preferably, the method for planning the agricultural machinery trajectory further includes:

[0012] In response to the fact that the current position of the agricultural machinery has not reached the starting point of the global path, the agricultural machinery plans a path from its current position to the starting point of the global path using the A* search algorithm based on dynamic constraints and the agricultural machinery pose information.

[0013] Preferably, the method for planning the agricultural machinery trajectory further includes:

[0014] In response to the collision detection result indicating that the agricultural machinery will not collide with the obstacle, the reference trajectory is determined as the agricultural machinery's travel trajectory.

[0015] Preferably, the agricultural machinery pose information also includes the speed of the agricultural machinery in its current state; the step of planning a path from the agricultural machinery's current position to the global path start point using the agricultural machinery pose information in response to the agricultural machinery's current position not reaching the global path start point includes:

[0016] The A* search algorithm based on dynamic constraints includes a first heuristic cost function and a first actual cost function.

[0017] The first heuristic cost function of the A* search algorithm based on dynamic constraints Determined by the following formula:

[0018] ;

[0019] in, This is used to characterize the path distance from the current position of the agricultural machinery to the first planning target point, taking into account dynamic constraints. The first planning target point is determined based on the distance and direction between the current position of the agricultural machinery and the starting point of the global path. Speed ​​used to characterize the current state of agricultural machinery Used to characterize the first preset weight;

[0020] The first actual cost function of the A* search algorithm based on dynamic constraints Determined by the following formula:

[0021] ;

[0022]

[0023] ;

[0024] in, Used to characterize the first penalty term. Used to characterize the second preset weight, The value used to characterize the difference between the current speed and the target speed of agricultural machinery. Used to characterize the second penalty term, Used to characterize the third preset weight, Used to characterize the fourth preset weight, Used to characterize the fifth preset weight, The sixth preset weight is used to characterize tv, which characterizes the rate of change of velocity in the current state of the agricultural machinery, and ta, which characterizes the rate of change of acceleration in the current state of the agricultural machinery. The rate of change of angular velocity is used to characterize the current state of the agricultural machinery, while tyaw is used to characterize the rate of change of the angle of the agricultural machinery's forward direction.

[0025] Preferably, the agricultural machinery pose information further includes the current velocity of the agricultural machinery, the heading angle of the agricultural machinery's forward direction, the current acceleration of the agricultural machinery, and the current angular velocity of the agricultural machinery. The process of planning the agricultural machinery's obstacle avoidance trajectory based on the collision detection result indicating a collision between the agricultural machinery and an obstacle, using the A* search algorithm based on dynamic constraints, the agricultural machinery pose information, and the obstacle information, includes:

[0026] The A* search algorithm based on dynamic constraints includes a second heuristic cost function and a second actual cost function.

[0027] The second heuristic cost function of the A* search algorithm based on dynamic constraints Determined by the following formula:

[0028] ;

[0029] in, This is used to characterize the path distance from the current position of the agricultural machinery to the second planned target point, taking into account dynamic constraints. The second planned target point is determined along the preset global path based on the planned line-of-sight distance. Speed ​​used to characterize the current state of agricultural machinery Used to characterize the seventh preset weight;

[0030] The second actual cost function of the A* search algorithm based on dynamic constraints Determined by the following formula:

[0031] ;

[0032] ;

[0033] ;

[0034] ;

[0035] in, Used to characterize the third penalty term, Used to characterize the eighth preset weight Used to characterize the second preset weight, The ninth preset weight is used to characterize the difference between the current position of the agricultural machinery and the distance between the path point on the reference trajectory corresponding to the current position of the agricultural machinery. The value used to characterize the difference between the current speed and the target speed of agricultural machinery. This is used to characterize the difference between the azimuth angle of the agricultural machinery's forward direction and the azimuth angle of the corresponding path point on the reference trajectory; Used to characterize the second penalty term, Used to characterize the third preset weight, Used to characterize the fourth preset weight, Used to characterize the fifth preset weight, The sixth preset weight is used to characterize tv, which characterizes the rate of change of velocity in the current state of the agricultural machinery; ta, which characterizes the rate of change of acceleration in the current state of the agricultural machinery; and tyaw, which characterizes the rate of change of the heading angle in the forward direction of the agricultural machinery. The rate of change of angular velocity used to characterize the current state of agricultural machinery; Used to characterize the fourth penalty term, Used to characterize the tenth preset weight Used to represent the distance between obstacles and agricultural machinery on the horizontal axis. The distance between the obstacle and the agricultural machinery on the horizontal axis is zero when it is greater than the safety threshold, or when the distance between the obstacle and the agricultural machinery on the vertical axis is greater than the detour trigger distance.

[0036] Preferably, the agricultural machinery pose information further includes the current timestamp of the agricultural machinery, and the method for planning the agricultural machinery trajectory further includes:

[0037] In response to the agricultural machinery's current timestamp falling within the planning period of the reference trajectory, the path point on the reference trajectory corresponding to the agricultural machinery's current timestamp is determined as the agricultural machinery's planning starting point.

[0038] This disclosure also relates to a planning system for agricultural machinery trajectories, the planning system for agricultural machinery trajectories comprising:

[0039] The acquisition module is used to acquire the preset global path, agricultural machinery pose information, and obstacle information;

[0040] The determination module is used to determine the starting point of the global path based on the preset global path; the agricultural machinery pose information includes the current position, speed, and orientation of the agricultural machinery;

[0041] The reference trajectory module is used to generate a reference trajectory along the preset global path using an interpolation method in response to the agricultural machinery's current position having reached the global path start point;

[0042] The collision test module actively performs collision detection on the reference trajectory based on the obstacle information.

[0043] The planning module is used to plan the obstacle avoidance trajectory of the agricultural machinery in response to the collision detection result indicating that the agricultural machinery will collide with the obstacle. This is done using the A* search algorithm based on dynamic constraints, the agricultural machinery pose information, and the obstacle information.

[0044] This disclosure also relates to an electronic device, including a memory, a processor, and a computer program stored in the memory and used to run on the processor, wherein the processor executes the computer program to implement the above-described method for planning agricultural machinery trajectories.

[0045] This disclosure also relates to a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-described method for planning agricultural machinery trajectories.

[0046] This disclosure also relates to a computer program product, including a computer program that, when executed by a processor, implements the above-described method for planning agricultural machinery trajectories.

[0047] Based on common knowledge in the field, the above-mentioned preferred conditions can be combined arbitrarily to obtain various preferred embodiments of this disclosure.

[0048] The positive and progressive effects of this disclosure are as follows:

[0049] This disclosure improves the performance of agricultural machinery in obstacle avoidance scenarios by adapting a preset global path to a reference trajectory and dynamic constraints using an A* search algorithm. This includes improving the efficiency of planning agricultural machinery obstacle avoidance trajectories and improving the smoothness of agricultural machinery obstacle avoidance trajectories. Attached Figure Description

[0050] Figure 1 A flowchart illustrating an exemplary embodiment of this disclosure of a method for planning the trajectory of agricultural machinery;

[0051] Figure 2 A schematic diagram of a preset global path for a method of planning agricultural machinery trajectories provided as an exemplary embodiment of this disclosure;

[0052] Figure 3 A flowchart illustrating an example of a method for planning agricultural machinery trajectories provided as an exemplary embodiment of this disclosure;

[0053] Figure 4 A structural diagram of an agricultural machinery trajectory planning system provided as an exemplary embodiment of this disclosure;

[0054] Figure 5 This is a schematic diagram of the structure of an electronic device provided as an exemplary embodiment of the present disclosure. Detailed Implementation

[0055] The present disclosure is further illustrated below by way of embodiments, but the present disclosure is not limited to the scope of the embodiments described herein.

[0056] The prefixes such as "first" and "second" used in this disclosure are merely for distinguishing different descriptive objects and do not limit the position, order, priority, quantity, or content of the described objects. The use of ordinal numbers and other prefixes used to distinguish descriptive objects in this disclosure does not constitute a limitation on the described objects. The description of the described objects is given in the claims or the context of the embodiments, and should not be construed as an unnecessary limitation. Furthermore, in the description of this embodiment, unless otherwise stated, "multiple" means two or more.

[0057] In this embodiment of the disclosure, the collection, storage, use, processing, transmission, provision, and disclosure of user personal information comply with relevant laws and regulations and do not violate public order and good morals.

[0058] Example 1

[0059] This embodiment provides a method for planning the trajectory of agricultural machinery. This method can be applied to the automatic driving of agricultural machinery for irrigation and pesticide spraying in orchards and vegetable fields, and to avoid obstacles and crops.

[0060] Figure 1 A flowchart illustrating a method for planning agricultural machinery trajectories, as provided in an exemplary embodiment of this disclosure, is shown below. Figure 1 As shown, the methods for planning agricultural machinery trajectories include:

[0061] S1. Obtain the preset global path, agricultural machinery pose information, and obstacle information.

[0062] The agricultural machinery position and orientation information includes the current position, speed, and orientation of the agricultural machinery.

[0063] refer to Figure 2 The preset global path is generally generated by a path planning algorithm. Based on the task objective (such as traversing the entire field) and static map information, it outputs a path from the starting point to the end point that covers the entire field.

[0064] Agricultural machinery pose information (i.e., the position and orientation of the agricultural machinery) usually comes from the positioning unit, which estimates the precise pose of the agricultural machinery in the global coordinate system in real time.

[0065] Obstacle information is obtained by detecting dynamic or static obstacles in the surrounding environment using sensors such as LiDAR, cameras, and millimeter-wave radar. After target detection, tracking, and fusion, the information such as the position, velocity, and shape of the obstacles in the global or local coordinate system is output.

[0066] S2. Determine the starting point of the global path based on the preset global path.

[0067] S3. In response to the fact that the current position of the agricultural machinery has reached the starting point of the global path, a reference trajectory is generated along the preset global path using an interpolation method.

[0068] Interpolation methods utilize pre-defined key path points (such as the planning start point, planning target point, and global path endpoint) and their potential associated state information (such as direction and speed) to construct a continuous and smooth function. This ensures that the generated trajectory accurately passes through these known points and meets certain smoothness requirements. Specifically, the interpolation method uses path parameters (such as normalized distance or time) as independent variables and employs mathematical tools such as linear interpolation and cubic splines to "fill" dense intermediate positions between adjacent points, thereby forming a complete reference trajectory that can be tracked. This reference trajectory not only connects discrete waypoints but also ensures the continuity of position, speed, and even acceleration to meet the feasibility requirements of agricultural machinery movement.

[0069] S4. Perform collision detection on the reference trajectory based on obstacle information.

[0070] In other words, given the reference trajectory and obstacle positions, collision detection discretizes the reference trajectory into a series of dense path points or line segments, and combines this with the geometry of the agricultural machinery to check whether these trajectory elements overlap or intrude into the area occupied by the obstacle during the movement. If the minimum distance between the trajectory pose of the agricultural machinery and the obstacle is less than the safety threshold (which can be set according to the actual situation) at any time, it is determined that there is a collision risk, thereby triggering the adjustment or replanning of the reference trajectory.

[0071] S5. In response to the collision detection result that the agricultural machinery will collide with the obstacle, the obstacle avoidance trajectory of the agricultural machinery is planned by using the A* search algorithm based on dynamic constraints, the agricultural machinery pose information and obstacle information.

[0072] In an optional implementation, the agricultural machinery pose information further includes the current velocity of the agricultural machinery, the heading angle of the agricultural machinery's forward direction, the current acceleration of the agricultural machinery, and the current angular velocity of the agricultural machinery. Step S5 includes:

[0073] The A* search algorithm based on dynamic constraints includes a second heuristic cost function and a second actual cost function.

[0074] The second heuristic cost function of the A* search algorithm based on dynamic constraints Determined by the following formula:

[0075] .

[0076] in, This is used to characterize the path distance from the current position of the agricultural machinery to the second planning target point, taking into account dynamic constraints. The second planning target point is determined along a preset global path based on the planned line-of-sight distance. Speed ​​used to characterize the current state of agricultural machinery Used to characterize the seventh preset weight. If the distance between the current position of the agricultural machinery and the second planning target point is less than the threshold (set according to actual conditions), the straight-line distance is used as the weight. Otherwise, the length of the Dubins curve (a shortest path planned in a two-dimensional plane for an agricultural machine with a minimum turning radius constraint) from the current position of the agricultural machinery to the second planning target point is used as the standard. .

[0077] The second practical cost function of the A* search algorithm based on dynamic constraints Determined by the following formula:

[0078] .

[0079] .

[0080] .

[0081] .

[0082] in, Used to characterize the third penalty term, Used to characterize the eighth preset weight Used to characterize the second preset weight, The ninth preset weight is used to represent the difference between the current position of the agricultural machinery and the path point on the reference trajectory corresponding to the current position of the agricultural machinery. The value used to characterize the difference between the current speed and the target speed of agricultural machinery. It is used to characterize the difference between the heading angle of the agricultural machinery and the heading angle of the corresponding path point on the reference trajectory. Used to characterize the second penalty term, Used to characterize the third preset weight, Used to characterize the fourth preset weight, Used to characterize the fifth preset weight, The sixth preset weight is used to characterize tv, which characterizes the rate of change of velocity in the current state of the agricultural machinery; ta, which characterizes the rate of change of acceleration in the current state of the agricultural machinery; and tyaw, which characterizes the rate of change of the heading angle in the forward direction of the agricultural machinery. The rate of change of angular velocity used to characterize the current state of agricultural machinery. Used to characterize the fourth penalty term, Used to characterize the tenth preset weight Used to represent the distance between obstacles and agricultural machinery on the horizontal axis. The distance between the obstacle and the agricultural machinery on the horizontal axis is zero when it is greater than the safety threshold, or when the distance between the obstacle and the agricultural machinery on the vertical axis is greater than the detour trigger distance.

[0083] It should be noted that the safety threshold and detour trigger distance can be set according to the actual situation.

[0084] The dynamically constrained A* search algorithm in this embodiment is an extension of the classic A* algorithm. It is used to solve path planning problems in the state space of agricultural machinery, simultaneously considering the current position and velocity, and satisfying system dynamic constraints (such as acceleration limits, maximum speed limits, control input limits, etc.). The core idea of ​​the dynamically constrained A* search algorithm is to extend path search from the traditional geometric space to the state space (including position, velocity, and other dynamic variables), and strictly adhere to the dynamic model during state transitions. The goal of path search is to find a feasible and cost-optimal trajectory from the initial state of the agricultural machinery to the target state.

[0085] This embodiment utilizes the A* search algorithm based on reference trajectory and dynamic constraints to adapt to a preset global path in agricultural machinery operation scenarios, improving the performance of agricultural machinery in obstacle avoidance scenarios. This includes increasing the efficiency of planning obstacle avoidance trajectories and improving the smoothness of the obstacle avoidance trajectory. Using time as a second heuristic function allows for rapid searching of paths that bring the agricultural machinery closer to the second planned target point, thereby reducing the number of path searches. A second actual cost function is set to ensure that the agricultural machinery gets as close as possible to the preset global path.

[0086] In an optional implementation, the method for planning agricultural machinery trajectories further includes:

[0087] S6. In response to the fact that the current position of the agricultural machinery has not reached the starting point of the global path, the path from the current position of the agricultural machinery to the starting point of the global path is planned by the A* search algorithm based on dynamic constraints and the agricultural machinery pose information.

[0088] In an optional implementation, the agricultural machinery pose information also includes the speed of the agricultural machinery in its current state. Step S6 includes:

[0089] The A* search algorithm based on dynamic constraints includes a first heuristic cost function and a first actual cost function.

[0090] The first heuristic cost function of the A* search algorithm based on dynamic constraints Determined by the following formula:

[0091] .

[0092] in, The path distance from the current position of the agricultural machinery to the first planning target point, considering dynamic constraints, is used to characterize the path distance. The first planning target point is determined based on the distance and direction between the current position of the agricultural machinery and the starting point of the global path. Speed ​​used to characterize the current state of agricultural machinery This is used to characterize the first preset weight. It should be noted that if the distance between the current position of the agricultural machinery and the first planned target point is less than a threshold (which can be set according to actual conditions), the straight-line distance is used as the threshold. Otherwise, the length of the Dubins curve (a shortest path planned in a two-dimensional plane for an agricultural machine with a minimum turning radius constraint) from the current position of the agricultural machinery to the first planning target point is used as the [length of the curve]. .

[0093] The first practical cost function of the A* search algorithm based on dynamic constraints Determined by the following formula:

[0094] ;

[0095]

[0096]

[0097] in, Used to characterize the first penalty term. Used to characterize the second preset weight, The value used to characterize the difference between the current speed and the target speed of agricultural machinery. Used to characterize the second penalty term, Used to characterize the third preset weight, Used to characterize the fourth preset weight, Used to characterize the fifth preset weight, The sixth preset weight is used to characterize tv, which characterizes the rate of change of velocity in the current state of the agricultural machinery, and ta, which characterizes the rate of change of acceleration in the current state of the agricultural machinery. The rate of change of angular velocity is used to characterize the current state of the agricultural machinery, while tyaw is used to characterize the rate of change of the angle of the agricultural machinery's forward direction.

[0098] In this implementation, if the agricultural machinery has not reached the global path start point, it is considered an open space scenario, and a path to the global path start point can be directly generated using the A* search algorithm based on dynamic constraints. Using time as the first heuristic function allows the agricultural machinery to quickly search for a path that approximates the first planned target point, thereby reducing the number of path searches. Since the agricultural machinery is currently not at the global path start point, there is no reference trajectory. At this time, the first actual cost function only needs to consider the speed cost, and path search results where the agricultural machinery is too close to obstacles are directly deleted.

[0099] In an optional implementation, the method for planning agricultural machinery trajectories further includes:

[0100] S7. In response to the collision detection result that the agricultural machinery will not collide with the obstacle, the reference trajectory is determined as the trajectory of the agricultural machinery.

[0101] In this embodiment, if the agricultural machinery does not collide with the obstacle, the reference trajectory is safe and usable, and this embodiment considers this situation as a path following scenario.

[0102] In an optional real-time mode, the agricultural machinery pose information also includes the current timestamp of the agricultural machinery, and the agricultural machinery trajectory planning method also includes:

[0103] S8. In response to the agricultural machinery's current timestamp being within the planning period of the reference trajectory, the path point on the reference trajectory corresponding to the agricultural machinery's current timestamp is determined as the agricultural machinery's planning starting point.

[0104] The following is a specific example to illustrate the agricultural machinery trajectory planning method of this embodiment. Figure 3 The flowchart for this example shows the core steps as follows:

[0105] S301. Obtain the preset global path, agricultural machinery pose information and obstacle information published by the upstream module.

[0106] The upstream module includes a path planning algorithm unit, a positioning unit, and a perception unit (including sensors such as lidar, cameras, and millimeter-wave radar) which are used to acquire preset global paths, agricultural machinery pose information, and obstacle information, respectively.

[0107] S302. Determine the starting point and target point of the plan. Plan the agricultural machinery's travel path in real time.

[0108] Specifically: The planning starting point is determined based on the agricultural machinery's position and orientation information and the local planning trajectory (i.e., the local trajectory of the reference trajectory). If there is no local planning trajectory or the current agricultural machinery's timestamp exceeds the timestamp of the last point of the local planning trajectory, the current position of the agricultural machinery is used as the planning starting point. Otherwise, the path point with the same timestamp on the local planning trajectory is selected as the planning starting point based on the current agricultural machinery's timestamp.

[0109] The planning target point is extracted along the preset global path according to the planned line-of-sight distance. If the current position of the agricultural machinery is not at the starting point of the global path, the planning target point is determined based on the relative position of the agricultural machinery to the starting point of the global path. If the relative position is far, the planning target point is determined along the direction of the relative position (i.e., the direction from the agricultural machinery to the starting point of the global path) according to the planned line-of-sight distance; if the relative position is close, the starting point of the global path is set as the planning target point. For example, if the relative distance between the agricultural machinery and the starting point of the global path is greater than 15 meters, the agricultural machinery is considered far from the starting point of the global path, and the planning target point is determined along the direction of the relative position according to the planned line-of-sight distance. If the relative distance between the agricultural machinery and the starting point of the global path is less than 15 meters, the agricultural machinery is considered close to the starting point of the global path, and the starting point of the global path is set as the planning target point.

[0110] S303. Generate a reference local trajectory along the preset global path using an interpolation method.

[0111] If there are obstacles on the preset global path, collision detection needs to be performed on the reference local path based on the obstacle's position. If a collision occurs on the reference trajectory, it is considered an obstacle avoidance scenario, and an obstacle avoidance trajectory is generated using the A* search algorithm based on dynamic constraints. If the reference trajectory is safe, it is considered a path following scenario.

[0112] If the current position of the agricultural machinery is not at the starting point of the global path, it is considered an open space scenario, and the A* search algorithm based on dynamic constraints is used to generate a path for the agricultural machinery to reach the starting point of the global path.

[0113] S304. A* search algorithm based on dynamic constraints is used for local path planning. The speed, angular velocity and other state information of the agricultural machinery are sampled. The current state of the agricultural machinery is recursively calculated to predict the state of the agricultural machinery at the next moment. The state search results are evaluated based on a heuristic cost function.

[0114] S305. Set different cost functions according to different operating scenarios to evaluate costs.

[0115] Specifically:

[0116] When encountering obstacle avoidance scenarios, set a penalty for agricultural machinery regarding its reference trajectory. This allows the agricultural machinery to conform as closely as possible to the preset global path.

[0117] ;

[0118] Among them, dd, dv and These are the differences between the current position, velocity, and orientation angle and the path points on the reference trajectory at the same timestamp. , and These are the weights of the corresponding distance, speed, and orientation angle difference penalty terms.

[0119] Set a penalty for the rate of change in agricultural machinery condition. To suppress changes in the condition of agricultural machinery:

[0120] ;

[0121] Among them, tv, ta, tyaw and These are the current state velocity, acceleration, and the rate of change of orientation angle and angular velocity. , , and These are the weights of the corresponding rate of change of velocity, rate of change of acceleration, rate of change of orientation angle, and penalty terms for the rate of change of angular velocity.

[0122] Penalties are set for the relative positions of agricultural machinery and obstacles. Keep agricultural machinery away from obstacles to avoid collisions:

[0123] ;

[0124] in, It is the distance of the obstacle in the lateral component of the agricultural machinery. This refers to the weight of the corresponding distance penalty term. When the distance of the obstacle in the lateral direction of the agricultural machinery exceeds the safety threshold, or the distance of the obstacle in the longitudinal direction of the agricultural machinery exceeds the detour trigger distance, It is 0.

[0125] Design a cost function for obstacle avoidance scenarios. for:

[0126] ;

[0127] Design a heuristic function for obstacle avoidance scenarios. for:

[0128] ;

[0129] in Let v be the position from the current state to the planned target point, and v be the velocity of the current state. This refers to the preset weights of the heuristic cost. When the distance between the current state of the agricultural machinery and the planned target point is less than a threshold, the straight-line distance is used as the weight. Otherwise, use the length of the Dubins curve from the current state position to the planned target point as... Using time as a heuristic cost allows the search to quickly approach the target planning point, thereby reducing the number of searches. Although this may lead to a greater likelihood of selecting a state with a larger value (v), the need for speed adjustment is weak in low-speed agricultural scenarios, so the impact on search results is relatively small.

[0130] When in an open space scenario, the heuristic function is the same as when in an obstacle avoidance scenario, but the cost function changes as follows:

[0131] Since the starting point of the global path has not been reached, there is no reference trajectory at this time. Considering only the cost of speed, i.e. the difference between the current vehicle speed and the target vehicle speed:

[0132] ;

[0133] because The value decreases, then set it again. This can cause excessive lateral offset when navigating around obstacles, so it is not recommended to set it in open space scenes. Instead, it sets a hard constraint on the safety threshold and directly deletes search results that are too close to the target location.

[0134] In obstacle avoidance scenarios, this example reduces the number of searches by approximately 80% and the algorithm execution time by approximately 50% compared to existing solutions, significantly improving real-time performance. This example designs different reasonable cost functions for different scenarios, resulting in a smoother trajectory during obstacle avoidance, eliminating vehicle head swaying, and reducing the turning angle change rate by approximately 40%. This example, combined with a local trajectory algorithm based on a preset global path, meets the requirements for full-coverage path motion and performs well in various operating scenarios, especially obstacle avoidance scenarios. In obstacle avoidance scenarios, the error between the agricultural machinery's movement deviation and the set safety deviation threshold is less than 0.3 meters.

[0135] Example 2

[0136] Corresponding to the aforementioned embodiments of the agricultural machinery trajectory planning method, this disclosure also provides embodiments of an agricultural machinery trajectory planning system.

[0137] Figure 4 A schematic diagram of a module for a planning system for agricultural machinery trajectories provided as an exemplary embodiment of this disclosure, as shown below. Figure 4 As shown, the agricultural machinery trajectory planning system includes:

[0138] Module 1 is used to acquire preset global paths, agricultural machinery pose information, and obstacle information.

[0139] Module 2 is used to determine the starting point of the global path based on a preset global path. The agricultural machinery's pose information includes its current position, speed, and orientation.

[0140] Reference trajectory module 3 is used to generate a reference trajectory along a preset global path by interpolation method in response to the agricultural machinery's current position reaching the global path start point.

[0141] Collision test module 4 actively performs collision detection on the reference trajectory based on obstacle information.

[0142] Planning module 5 is used to plan the obstacle avoidance trajectory of the agricultural machinery in response to the collision detection result that the agricultural machinery will collide with the obstacle. It uses the A* search algorithm based on dynamic constraints, the agricultural machinery pose information and obstacle information.

[0143] In an optional implementation, the planning module 5 is further configured to, in response to the fact that the current position of the agricultural machinery has not reached the global path start point, plan the path of the agricultural machinery from the current position of the agricultural machinery to the global path start point using the A* search algorithm based on dynamic constraints and the agricultural machinery pose information.

[0144] In an optional implementation, the planning module 5 is further configured to determine the reference trajectory as the travel trajectory of the agricultural machinery in response to a collision detection result indicating that the agricultural machinery will not collide with the obstacle.

[0145] In an optional implementation, the agricultural machinery pose information also includes the speed of the agricultural machinery in its current state. The A* search algorithm based on dynamic constraints is used to plan the path of the agricultural machinery from its current position to the starting point of the global path using the agricultural machinery pose information. The A* search algorithm based on dynamic constraints includes a first heuristic cost function and a first actual cost function.

[0146] The first heuristic cost function of the A* search algorithm based on dynamic constraints Determined by the following formula:

[0147] .

[0148] in, The path distance from the current position of the agricultural machinery to the first planning target point, considering dynamic constraints, is used to characterize the path distance. The first planning target point is determined based on the distance and direction between the current position of the agricultural machinery and the starting point of the global path. Speed ​​used to characterize the current state of agricultural machinery Used to characterize the first preset weight.

[0149] The first practical cost function of the A* search algorithm based on dynamic constraints Determined by the following formula:

[0150] ;

[0151]

[0152] ;

[0153] in, Used to characterize the first penalty term. Used to characterize the second preset weight, The value used to characterize the difference between the current speed and the target speed of agricultural machinery. Used to characterize the second penalty term, Used to characterize the third preset weight, Used to characterize the fourth preset weight, Used to characterize the fifth preset weight, The sixth preset weight is used to characterize tv, which characterizes the rate of change of velocity in the current state of the agricultural machinery, and ta, which characterizes the rate of change of acceleration in the current state of the agricultural machinery. The rate of change of angular velocity is used to characterize the current state of the agricultural machinery, while tyaw is used to characterize the rate of change of the angle of the agricultural machinery's forward direction.

[0154] The agricultural machinery pose information also includes the current velocity, the heading angle of the agricultural machinery's forward direction, the current acceleration, and the current angular velocity of the agricultural machinery. The A* search algorithm based on dynamic constraints, which plans the agricultural machinery's obstacle avoidance trajectory using the agricultural machinery pose information and obstacle information, includes a second heuristic cost function and a second actual cost function.

[0155] The second heuristic cost function of the A* search algorithm based on dynamic constraints Determined by the following formula:

[0156] .

[0157] in, This is used to characterize the path distance from the current position of the agricultural machinery to the second planning target point, taking into account dynamic constraints. The second planning target point is determined along a preset global path based on the planned line-of-sight distance. Speed ​​used to characterize the current state of agricultural machinery Used to characterize the seventh preset weight.

[0158] The second practical cost function of the A* search algorithm based on dynamic constraints Determined by the following formula:

[0159] .

[0160] .

[0161] .

[0162] .

[0163] in, Used to characterize the third penalty term, Used to characterize the eighth preset weight Used to characterize the second preset weight, The ninth preset weight is used to represent the difference between the current position of the agricultural machinery and the path point on the reference trajectory corresponding to the current position of the agricultural machinery. The value used to characterize the difference between the current speed and the target speed of agricultural machinery. It is used to characterize the difference between the heading angle of the agricultural machinery and the heading angle of the corresponding path point on the reference trajectory. Used to characterize the second penalty term, Used to characterize the third preset weight, Used to characterize the fourth preset weight, Used to characterize the fifth preset weight, The sixth preset weight is used to characterize tv, which characterizes the rate of change of velocity in the current state of the agricultural machinery; ta, which characterizes the rate of change of acceleration in the current state of the agricultural machinery; and tyaw, which characterizes the rate of change of the heading angle in the forward direction of the agricultural machinery. The rate of change of angular velocity used to characterize the current state of agricultural machinery. Used to characterize the fourth penalty term, Used to characterize the tenth preset weight Used to represent the distance between obstacles and agricultural machinery on the horizontal axis. The distance between the obstacle and the agricultural machinery on the horizontal axis is zero when it is greater than the safety threshold, or when the distance between the obstacle and the agricultural machinery on the vertical axis is greater than the detour trigger distance.

[0164] In an optional real-time mode, the agricultural machinery pose information also includes the current timestamp of the agricultural machinery. The reference trajectory module 3 is also used to determine the path point on the reference trajectory corresponding to the current timestamp of the agricultural machinery as the planning start point of the agricultural machinery in response to the current timestamp of the agricultural machinery being within the planning period of the reference trajectory.

[0165] For the system embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to in the description of the method embodiments. The system embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separate, and the components as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this disclosure according to actual needs.

[0166] Example 3

[0167] Figure 5 This is a schematic diagram of the structure of an electronic device according to an example embodiment of the present disclosure. The electronic device includes a memory, a processor, and a computer program stored in the memory and used to run on the processor. When the processor executes the computer program, it implements the agricultural machinery trajectory planning method described in any of the above embodiments. Figure 5 The electronic device 90 shown is merely an example and should not impose any limitation on the functionality and scope of use of the embodiments disclosed herein.

[0168] like Figure 5 As shown, the electronic device 90 can be manifested as a general-purpose computing device, such as a server device. The components of the electronic device 90 may include, but are not limited to: at least one processor 91, at least one memory 92, and a bus 93 connecting different system components (including memory 92 and processor 91).

[0169] Bus 93 includes a data bus, an address bus, and a control bus.

[0170] The memory 92 may include volatile memory, such as random access memory (RAM) 921 and / or cache memory 922, and may further include read-only memory (ROM) 923.

[0171] The memory 92 may also include a program tool 925 (or utility) having a set (at least one) program module 924, such program module 924 including but not limited to: an operating system, one or more application programs, other program modules, and program data, each or some combination of these examples may include an implementation of a network environment.

[0172] The processor 91 executes various functional applications and data processing by running computer programs stored in the memory 92, such as the agricultural machinery trajectory planning method provided in any of the above embodiments.

[0173] Electronic device 90 can also communicate with one or more external devices 94 (e.g., keyboard, pointing device, etc.). This communication can be performed through input / output (I / O) interface 95. Furthermore, electronic device 90 can also communicate with one or more networks (e.g., local area network (LAN), wide area network (WAN), and / or public network, such as the Internet) via network adapter 96. As shown, network adapter 96 communicates with other modules of electronic device 90 via bus 93. It should be understood that, although not shown in the figure, other hardware and / or software modules can be used in conjunction with electronic device 90, including but not limited to: microcode, device drivers, redundant processors, external disk drive arrays, RAID (disk array) systems, tape drives, and data backup storage systems.

[0174] It should be noted that although several units / modules or sub-units / modules of the electronic device have been mentioned in the detailed description above, this division is merely exemplary and not mandatory. In fact, according to embodiments of this disclosure, the features and functions of two or more units / modules described above can be embodied in one unit / module. Conversely, the features and functions of one unit / module described above can be further divided and embodied by multiple units / modules.

[0175] Example 4

[0176] This disclosure also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the agricultural machinery trajectory planning method provided in any of the above embodiments.

[0177] The readable storage medium may be more specifically adopted, including but not limited to: portable disk, hard disk, random access memory, read-only memory, erasable programmable read-only memory, optical storage device, magnetic storage device, or any suitable combination thereof.

[0178] Example 5

[0179] This disclosure also provides a computer program product, including a computer program that, when executed by a processor, implements the agricultural machinery trajectory planning method described in any of the above embodiments.

[0180] The program code for executing the computer program product of this disclosure can be written in any combination of one or more programming languages, and the program code can be executed entirely on a user device, partially on a user device, as a stand-alone software package, partially on a user device and partially on a remote device, or entirely on a remote device.

[0181] While specific embodiments of this disclosure have been described above, those skilled in the art should understand that these are merely illustrative examples, and the scope of protection of this disclosure is defined by the appended claims. Those skilled in the art can make various changes or modifications to these embodiments without departing from the principles and essence of this disclosure, but all such changes and modifications fall within the scope of protection of this disclosure.

Claims

1. A method for planning a trajectory of an agricultural machine, characterized in that, The agricultural machine trajectory planning method comprises: obtaining a preset global path, agricultural machine pose information and obstacle information; determining a global path starting point according to the preset global path; the agricultural machine pose information comprises an agricultural machine current position, speed and orientation; in response to the agricultural machine current position reaching the global path starting point, generating a reference trajectory along the preset global path by an interpolation method; performing collision detection on the reference trajectory according to the obstacle information; in response to a collision detection result being that the agricultural machine will collide with the obstacle, planning an agricultural machine obstacle avoidance trajectory by an A* search algorithm based on dynamics constraints, the agricultural machine pose information and the obstacle information.

2. The method of claim 1, wherein, The agricultural machine trajectory planning method further comprises: in response to the agricultural machine current position not reaching the global path starting point, planning a path from the agricultural machine current position to the global path starting point by the A* search algorithm based on dynamics constraints and the agricultural machine pose information.

3. The method of claim 1, wherein, The agricultural machine trajectory planning method further comprises: in response to the collision detection result being that the agricultural machine will not collide with the obstacle, determining the reference trajectory as an agricultural machine travel trajectory.

4. The method for planning a farm machine trajectory according to claim 2, wherein, The agricultural machine pose information further comprises a speed of an agricultural machine current state; the response to the agricultural machine current position not reaching the global path starting point, planning a path from the agricultural machine current position to the global path starting point by the A* search algorithm based on dynamics constraints and the agricultural machine pose information comprises: wherein the A* search algorithm based on dynamics constraints comprises a first heuristic cost function and a first actual cost function; The first heuristic cost function of the dynamic constraint-based A* search algorithm is determined by the following equation: ； wherein, a first planning target point is determined according to the distance and direction of the relative position between the current position of the agricultural machine and the starting point of the global path, a speed of the current state of the agricultural machine is represented, a first preset weight is represented; The first actual cost function of the dynamic constraint based A* search algorithm is determined by the following equation: ； ； wherein, for representing the first penalty term, for representing the second preset weight, for representing the difference between the speed of the current state of the agricultural machine and the target vehicle speed, for representing the second penalty term, for representing the third preset weight, for representing the fourth preset weight, for representing the fifth preset weight, for representing the sixth preset weight, tv for representing the rate of change of speed of the current state of the agricultural machine, ta for representing the rate of change of acceleration of the current state of the agricultural machine, for representing the rate of change of angular velocity of the current state of the agricultural machine, tyaw for representing the rate of change of heading angle of the agricultural machine.

5. The method of claim 1, wherein, The agricultural machine pose information further comprises a speed of an agricultural machine current state, an agricultural machine forward direction orientation angle, an acceleration of an agricultural machine current state, an angular velocity of an agricultural machine current state, and the response to the collision detection result being that the agricultural machine will collide with the obstacle, planning an agricultural machine obstacle avoidance trajectory by the A* search algorithm based on dynamics constraints, the agricultural machine pose information and the obstacle information comprises: wherein the A* search algorithm based on dynamics constraints comprises a second heuristic cost function and a second actual cost function; The second heuristic cost function of the dynamics-constrained A* search algorithm is determined by the equation: ； wherein, a path distance from the current position of the agricultural machine to a second planning target point considering dynamic constraints, the second planning target point being determined along the preset global path by a planning horizon, a speed representing a current state of the agricultural machine, a seventh preset weight; The second actual cost function of the dynamics-constrained A* search algorithm is determined by the equation: ； ； ； ； wherein, for representing the third penalty term, for representing the eighth preset weight, for representing the second preset weight, for representing the ninth preset weight, dd for representing the difference between the distance of the current position of the agricultural machine and the path point corresponding to the current position of the agricultural machine on the reference trajectory, for representing the difference between the speed of the current state of the agricultural machine and the target vehicle speed, for representing the difference between the heading angle of the current state of the agricultural machine and the heading angle of the corresponding path point on the reference trajectory; for representing the second penalty term, for representing the third preset weight, for representing the fourth preset weight, for representing the fifth preset weight, for representing the sixth preset weight, tv for representing the rate of change of the speed of the current state of the agricultural machine, ta for representing the rate of change of the acceleration of the current state of the agricultural machine, and tyaw for representing the rate of change of the heading angle of the agricultural machine, for representing the rate of change of the angular velocity of the current state of the agricultural machine; for representing the fourth penalty term, for representing the tenth preset weight, for representing the distance between the obstacle and the agricultural machine in the horizontal coordinate, zero when the distance between the obstacle and the agricultural machine in the horizontal coordinate is greater than the safety threshold or the distance between the obstacle and the agricultural machine in the vertical coordinate is greater than the detour triggering distance.

6. The method of claim 1, wherein, The agricultural machine pose information further comprises an agricultural machine current timestamp, and the agricultural machine trajectory planning method further comprises: in response to the agricultural machine current timestamp being within a planning period of the reference trajectory, determining a path point corresponding to the agricultural machine current timestamp on the reference trajectory as a planning starting point of the agricultural machine.

7. A system for planning a trajectory of an agricultural machine, characterized in that The agricultural machine trajectory planning system comprises: an acquisition module configured to acquire a preset global path, agricultural machine pose information and obstacle information; a determination module configured to determine a global path starting point according to the preset global path; the agricultural machine pose information comprises an agricultural machine current position, speed and orientation; a reference trajectory module configured to generate a reference trajectory along the preset global path by an interpolation method in response to the agricultural machine current position reaching the global path starting point; a collision test module configured to perform collision detection on the reference trajectory according to the obstacle information; a planning module configured to plan an agricultural machine obstacle avoidance trajectory by an A* search algorithm based on dynamics constraints, the agricultural machine pose information and the obstacle information in response to a collision detection result being that the agricultural machine will collide with the obstacle.

8. An electronic device comprising a memory, a processor, and a computer program stored on the memory for running on the processor, characterized in that, The computer program is executed by the processor to implement the agricultural machine track planning method in any one of claims 1 to 6.

9. A computer readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to implement the agricultural machine track planning method in any one of claims 1 to 6.

10. A computer program product comprising a computer program, characterized in that, The computer program is executed by the processor to implement the agricultural machine track planning method in any one of claims 1 to 6.

Images