A subarea path planning method for weeding operation

Abstract

This invention discloses a zoned path planning method for weeding operations, belonging to the field of intelligent agricultural machinery control technology. The method includes: constructing a map of the work environment including field boundaries and static obstacles, and performing safety buffer expansion on the obstacles; discretizing the work area into a grid map, generating a sequence of target points using a serpentine sorting method, dividing it into two independent task zones according to the crop row direction, and assigning them to two robots, prohibiting cross-zone travel through regional constraints; using a four-adjacent motion model for path planning and introducing a turning penalty term to ensure the path moves regularly along the crop row direction; real-time detection of dynamic obstacles during execution, triggering forward rolling replanning when there is a collision risk, generating a local avoidance path; finally, post-processing smoothing of the path, and rounding the corners to the minimum turning radius near the field edge and obstacle areas. This invention achieves strictly zoned operation between two robots, avoiding path intersections and collisions, and ensuring the continuity and safety of weeding operations.

Description

Technical Field

[0001] This invention relates to the field of intelligent agricultural machinery control technology, specifically a method for zoning path planning in weeding operations. Background Technology

[0002] Inter-row weeding operations typically require agricultural machinery to travel strictly in the direction of the crop rows to minimize accidental trampling on the crops, such as inter-row weeding operations in large-scale soybean fields.

[0003] Traditional full-coverage path planning methods, such as ox-plowing paths, can achieve area coverage, but in scenarios with many obstacles or large plots, path redundancy is significant, resulting in low operational efficiency. With the development of multi-robot collaborative operations, existing methods often lack effective task allocation and path constraint mechanisms, easily leading to problems such as inter-robot path intersections, target conflicts, cross-area crossings, and long-distance ineffective movements. This not only reduces overall operational efficiency but may also cause mutual interference or even collision risks between robots. Furthermore, dynamic obstacles are common in field environments, such as other robots, personnel, vehicles, or temporary debris. Traditional methods mostly generate paths based on static environments, making it difficult to effectively handle sudden encroachments during execution and lacking dynamic adjustment capabilities. Once the path ahead is blocked, the robot may become stuck or repeatedly fail to plan, severely impacting the continuity and safety of the operation. Summary of the Invention

[0004] The purpose of this invention is to provide a zoned path planning method for weeding operations to solve the above-mentioned problems.

[0005] The technical solution of this invention is: A method for zoning path planning in weeding operations includes the following steps: Construct a work environment map, which includes: field boundaries, work area, and static obstacles within the work area; perform a safety buffer expansion on the static obstacles, with the expansion distance being the sum of the robot radius and the safety margin; The work area is discretized into a grid map, and a sequence of target points to be covered is generated using a snake sorting algorithm. The sequence of target points is then divided into multiple independent task partitions according to the crop row direction. These multiple independent task partitions are assigned to their respective robots, and a region constraint function is used to prevent paths from crossing the partition boundaries.

[0006] The grid-based path planning method plans a collision-free path for each robot from its current position to the next target point within its assigned zone. The path planning method adopts a four-adjacent motion model, and node expansion is only allowed in four directions: horizontal and vertical. A turning penalty term is introduced, which increases the path cost when the direction of movement changes for two consecutive steps. The resulting path strictly follows the crop row direction, greatly reducing accidental crushing of crops, maintaining path regularity, and encouraging straight-line travel. During the process of the robot executing a collision-free path, the system detects in real time whether there are dynamic obstacles on the path ahead. The dynamic obstacles include the real-time positions of other robots and preset moving obstacles. If a collision risk is detected, a rolling replanning mechanism is triggered to re-search for feasible paths using the forward distance as a window. The subsequent planned paths are then updated with the feasible paths. The updated planned path is post-processed and smoothed, including removing collinear redundant points, straightening straight lines, and rounding corners with minimum turning radius only in the field boundary area and / or near obstacles, to generate the final executable path. The field boundary area is a strip-shaped area inside the boundary formed by offsetting a preset threshold into the work area based on the field boundary. The preset threshold is determined according to the robot's minimum turning radius and / or safety margin to ensure coverage efficiency. At the same time, the zoning method ensures that the work areas of the two robots do not overlap.

[0007] Furthermore, the number of independent task partitions is set to two, corresponding to the first robot and the second robot respectively. The snake sorting algorithm includes the following steps: According to the set crop row direction, the robot is divided into grid columns and moves back and forth along the crop row direction within each column. It also thins out the work area in the direction of row change according to the work width. The task partitioning method is as follows: the field is divided into two parts along the crop row direction. The first robot is responsible for the lower half of the field, and the second robot is responsible for the upper half of the field, ensuring that the path of each robot is strictly limited to its own partition.

[0008] Furthermore, the rolling replanning mechanism includes: obtaining the current time and updating the dynamic obstacle list; after planning a new path, advancing point by point along the path and checking whether each path point is occupied by a dynamic obstacle; if occupied, stopping execution and recording the number of blockages; when the number of blockages exceeds a threshold, marking the current target point as unreachable; if not occupied, updating the robot's current position and marking the grid within the coverage area as covered. This effectively avoids dynamic obstacles without interrupting operations, ensuring the continuity and safety of the operation.

[0009] Furthermore, the logic for marking the current target point as unreachable is as follows: for a target grid, if both the first robot and the second robot fail to reach it, or if a target grid is located within the robot's partition but the robot fails to plan multiple times, then the grid is removed from the set to be covered. This avoids the robot getting stuck in an infinite loop and deadlock due to long-term obstruction by obstacles.

[0010] Furthermore, during the minimum turning radius rounding process, an arc transition segment satisfying the minimum turning radius constraint is constructed based on the turning direction of adjacent path segments. Discretized collision detection is then performed on the arc transition segment and the line segments connecting it to the original path. When the detection result meets the safety constraints, the arc transition segment replaces the inflection point in the original polyline path; otherwise, the original polyline path is retained. Under the premise of satisfying the kinematic constraints of agricultural machinery, a smooth and traceable operating path is generated, while collision detection ensures the safety of the rounded path.

[0011] Furthermore, the safety margin is a configurable parameter used to increase the gap between the robot and obstacles beyond its physical radius. The value ranges from 0.2 to 0.8 meters. By flexibly configuring this parameter, operational efficiency and safety can be balanced in different field environments to meet the safety requirements of different operating environments.

[0012] Furthermore, the first and second robots employ an alternating advancement strategy. In each iteration, the robot closer to its current target point is prioritized for path planning and execution to balance the progress of the two robots' operations. Through this alternating advancement strategy, the progress of the two robots' operations is dynamically balanced, preventing one robot from being idle for extended periods and improving overall collaboration efficiency.

[0013] Furthermore, during the robot's execution along the final executable path, a coverage neighborhood is determined by a circular area centered on the current execution path point and with a radius equal to half the width of the spray pattern. Grids falling within this coverage neighborhood and located in the set to be covered are marked as covered. This spray pattern-based coverage status update mechanism allows the path point to complete the coverage task without strictly covering the center of each grid, further improving operational efficiency.

[0014] Furthermore, the selection of the next target point adopts a combination of sequential priority and nearest-point supplementation, including the following steps: prioritizing selection according to the target sequence of the current partition after serpentine sorting; when the target sequence of the current partition is exhausted, selecting the target point closest to the current robot position from the uncovered set of the current partition as the next task target. This ensures the orderliness of overall coverage while enabling flexible supplementation in local areas, avoiding long-distance empty runs by the robot.

[0015] Compared with the prior art, the beneficial effects of the present invention are: This invention completely isolates the working paths of two robots spatially by dividing the crop row direction into independent zones and setting regional constraints, eliminating the risk of path intersections and collisions at the source. Based on this, a path planning method combining a four-adjacent motion model and a turning penalty term is employed to ensure that the generated path direction strictly aligns with the crop row and maintains a long straight line, meeting the core agronomical requirement of crop protection during inter-row weeding. When dynamic obstacles appear, a rolling replanning is triggered using the forward distance as a window to generate a local alternative path, allowing the robot to flexibly avoid moving obstacles while adhering to zone constraints and row alignment. Furthermore, minimum turning radius rounding is applied only in areas near the field edge or obstacles, ensuring that straight-line travel within the field is unaffected by any smoothing while satisfying agricultural machinery kinematic constraints. This effectively solves the problem of work interruptions caused by path intersections, frequent turns, and dynamic obstacles in multi-machine collaborative operations, significantly improving the continuity and safety of operations. Attached Figure Description

[0016] Figure 1 This is an overall flowchart of the partition path planning method of the present invention.

[0017] Figure 2 The present invention provides a working environment map and a task partitioning diagram.

[0018] Figure 3 This is a schematic diagram of the path dynamic obstacle avoidance of the present invention.

[0019] Figure 4 This is a schematic diagram illustrating the full coverage of dual-machine collaborative operation according to the present invention. Detailed Implementation

[0020] The following is combined Figures 1 to 4 The specific embodiments of the present invention will be described in detail below.

[0021] Example A method for zoning path planning in weeding operations includes the following steps: Construct a work environment map, which includes: field boundaries, work area, and static obstacles within the work area; perform a safety buffer expansion on the static obstacles, with the expansion distance being the sum of the robot radius and the safety margin; The `full_coverage_path_planning_dual` function in the execution code is used. First, the field boundaries `ox` and `oy` are defined, the grassland operation area `grass_areas` (e.g., coordinate range [-10, 60, -10, 60]) and the static obstacles `obstacle_in_grass` within the grassland (used to simulate field facilities / ditches / stockpiles, etc.) are defined.

[0022] Create a planner AStarPlanner. During initialization, based on the set safety margin OBSTACLE_CLEARANCE_M (e.g., 0.8 meters) and robot radius rr (e.g., 1.0 meters), buffer and expand obstacles. To ensure path planning safety, rectangular obstacles in the field are expanded, and the expanded boundaries satisfy: . .

[0023] in, For the original obstacle boundary, This represents the expanded barrier boundary. The radius of the robot; For safety margin.

[0024] Subsequently, a raster map was created based on the field boundaries, with arbitrary coordinates. With grid coordinates The correspondence is as follows: .

[0025] Where (x, y) are the continuous coordinates of any point within the working area; x is the discrete raster index corresponding to this point; min With y min , respectively, represent the minimum values ​​of the field boundaries in the X and Y directions; r is the resolution of the raster map.

[0026] The entire grassland operation area is discretized into a grid. All non-obstacle grid cells are traversed to generate a set of uncovered_grass_cells to be covered. The `_build_serpentine_order` function is called to perform a serpentine sorting of the grid cells column-wise according to the crop row direction `CROP_ROW_DIRECTION="y"` (i.e., along the Y-axis). The sorting logic is as follows: using a column (X index) as a lane, the indices of all rows in that column are collected. Then, the traversal order of the rows is determined by the parity of the lane, thus achieving round-trip coverage.

[0027] At the same time, combined with the effective working width of the machine Crop row spacing Set the thinning step size in the line break direction. ,in This indicates the number of raster cells between adjacent columns of retained target points. This represents the raster map resolution. By using this thinning step size, only target points that meet the step size sampling conditions are retained, ensuring that the target point spacing matches the actual coverage capacity of the equipment, reducing redundant coverage and frequent backtracking between adjacent paths, and improving the efficiency of coverage path planning. The thinning step size formula is as follows:

[0028] .

[0029] That is, only target points that meet the step size sampling conditions are retained to reduce repeated backtracking.

[0030] Subsequently, task partitioning is performed based on the crop row direction: along the Y-axis, the median Y-coordinate (mid) of all grid cells to be covered is calculated. Grid cells with Y-coordinates less than or equal to mid are assigned to the first robot (StartRobot, red path), and grid cells with Y-coordinates greater than mid are assigned to the second robot (GoalRobot, blue path). To ensure that the second robot starts its work from the top of the region, its target list is processed in reverse order. Simultaneously, strict path partitioning check functions `region_start(iy<=mid)` and `region_goal(iy>mid)` are defined. In all subsequent A* planning, the `planner.region_checker` property prevents path nodes from crossing partition lines.

[0031] The grid-based path planning method calls the `planner.planning` method for each robot when it needs to move to the next target point (tx, ty). In this method, the motion model `self.motion` is configured to only have four directions (up, down, left, and right), disabling diagonal movement to ensure path regularity. The grid search with turning penalties during node expansion uses the following evaluation function:

[0032] .

[0033] in, For nodes The overall evaluation value, From the starting point to the node The cumulative actual cost, For nodes Heuristic cost estimation to the target node.

[0034] Among them, the cumulative actual cost satisfy: .

[0035] In the formula: For nodes The parent node, For the current expansion node, For the adjacent nodes to be expanded; For the node Move to node The cost of step size; This is the turning penalty coefficient; The function for determining the turn is expressed as follows: .

[0036] That is, when two consecutive steps involve a change in direction, it is determined that a turn has occurred and a turning penalty is applied; when two consecutive steps involve the same direction, no turning penalty is applied. This determination method is similar to the method in the code that compares the direction of the previous step. With the current expansion direction The implementation of whether to include turn_penalty is consistent, depending on whether they are the same. When a node is expanded, if the current movement direction is inconsistent with the direction from the parent node to the current node, an additional cost TURN_PENALTY is added to suppress frequent turning and make the path more in line with the straight-line operation habits of agricultural machinery.

[0037] Step size cost satisfy: .

[0038] in, This is the diagonal movement cost adjustment coefficient. In the default implementation, a four-adjacent motion model is used, allowing movement only in four directions: up, down, left, and right, with a step cost of 1 in each direction. By adding this additional cost, frequent turning is suppressed, making the path more consistent with the straight-line operation habits of agricultural machinery.

[0039] Local path replanning for dynamic obstacles includes dynamic obstacle no-entry region construction, forward collision detection, and local alternative path generation, and satisfies the following: for any path point to be detected... If it is related to the center of the dynamic obstacle The distance satisfies .

[0040] Then the path point is determined to be within the local restricted area of ​​the dynamic obstacle, where, The equivalent radius of the robot is given. For safety margin, The equivalent radius of the dynamic obstacle; the forward look-ahead distance of the current planned path. Collision detection is performed on the path segments within the range; if a forward-looking path point exists... satisfy If a collision risk is detected on the current path within the foreseeable range, a local path replanning will be triggered; where, The position of the dynamic obstacle at a future time. For the robot's movement speed, For the current moment; during local path replanning, the robot's current position is used as the local starting point, and the current task target point is used as the local ending point. While preserving static obstacle constraints and partition constraints, the grid search is re-executed to generate a local alternative path to avoid dynamic obstacles. During local path execution, the time updates of adjacent execution points satisfy: .

[0041] in, and These are adjacent execution points on the local path. and These represent the times when the corresponding execution point is reached; if the updated next execution point falls into the local restricted area of ​​the dynamic obstacle again, the current local path is interrupted and the local path is replanned again until the current task target point is reached or the preset replanning limit is reached.

[0042] When executing at path point (x, y), the function `mark_covered_from_path` is called. This function centers on the path point and extends the spray pattern by half the width of the spray pattern. / 2) Using the radius r_cells, calculate all raster cells falling within this circle. By traversing all raster cells within the radius r_cells, if a raster cell is in the set to be covered, remove it and add it to the covered set covered_cells. This simulates the working width of weeding equipment in actual operations, allowing pathpoints to complete the covering task without strictly covering the center of each raster cell.

[0043] The spray width of the weeding implement refers to the effective coverage width of the weeding implement in one operation, perpendicular to the robot's direction of travel, as the robot moves along the path. That is, when the robot moves along the path, it can process weeds within a certain range on both sides of the path. Therefore, half of this width is used as the coverage neighborhood radius to determine whether the target grid has been covered.

[0044] Once an initial path from the starting point to the destination is planned, redundant points with the same direction are removed using `_remove_collinear_points`, simplifying the path. This function converts path points into raster indices and then retains points whose direction changes.

[0045] The `_round_corners_min_turn` function is called to round the corners. Within the rounding function, the inflection points in the original polyline path are rounded. Select its adjacent path points and Construct unit vectors for the direction of entry into the curve. and the unit vector of the exit direction And calculate the turning angle at the inflection point according to the following formula:

[0046] .

[0047] In the formula Indicates the robot's direction of motion at the inflection point. The turning angle at that point.

[0048] Based on the preset minimum turning radius of the agricultural machinery , calculation The distance between the tangent points on both sides of the inflection point when performing corner rounding: .

[0049] in, This indicates that the minimum turning radius is met. Under the condition, from the inflection point Retreat along the entry and exit curve paths respectively to determine the distances required to reach the tangent point.

[0050] The maximum usable tangent distance is determined based on the actual lengths of the adjacent path segments on both sides of the inflection point. When the following conditions are met: When this occurs, it indicates that the length of the path segments on both sides of the inflection point is sufficient to accommodate a radius not less than [a certain value]. The circular transition section, at this time with As the radius of the fillet, an arc segment is generated; when If the path segment length at the inflection point is insufficient to construct an arc segment that satisfies the minimum turning radius constraint, then the inflection point will not be rounded, and the original polyline path will be retained or the inflection point will be used as a candidate point for subsequent local replanning.

[0051] After determining the center of the arc, The discrete points of the circular arc are generated using the angle sampling step size, where... This represents the arc length step size for discrete sampling of the circular arc. This represents the increment of the central angle corresponding to adjacent arc sampling points. Subsequently, point-by-point collision detection is performed on the arc sampling points and the straight line segments connecting them to the original path. When the detection results simultaneously satisfy obstacle safety constraints, partition constraints, and minimum turning radius constraints, the original inflection point is replaced by an arc segment; otherwise, the original broken line path is retained. This achieves path rounding processing that satisfies the kinematic constraints of agricultural machinery under feasible conditions.

[0052] In the main loop, the priority of each robot is dynamically determined by comparing the grid distances of the two robots to their respective next target points. This alternating approach balances the progress of the two robots' work, preventing one robot from idling for an extended period while waiting for the other.

[0053] In each iteration, the next target point is first selected for each robot from its respective partition list (if the list is exhausted, the nearest point within the current partition is selected from the globally uncovered set). Then, based on the distance comparison results, advance_one_side is called sequentially to perform path planning, dynamic obstacle avoidance, coverage marking, and position update.

[0054] Finally, when the uncovered set is empty or the maximum number of iterations is reached, the planning ends, and the complete path point sequences of the two robots, all_path_x_start, all_path_y_start, all_path_x_goal, and all_path_y_goal, as well as the trajectory data of external dynamic obstacles, are output for subsequent analysis or visualization.

[0055] This embodiment achieves conflict-free collaboration between two robots through task partitioning and regional constraint mechanisms: the work area is divided into two independent partitions, upper and lower, based on the crop row direction, and a strict path region checking function is set for each robot to ensure that path planning nodes cannot cross the partition boundaries. This mechanism fundamentally avoids the risk of path intersection and potential collisions between the two robots. Simultaneously, combined with an alternating advancement strategy to dynamically balance the progress of the two robots, it significantly improves the safety and reliability of multi-robot collaborative operations and solves the inefficiency problem caused by path overlap in traditional multi-robot systems.

[0056] A path planning mechanism combining a four-neighbor motion model and a turning penalty is employed to generate strictly regular paths along crop rows. The algorithm disables diagonal movement, allowing expansion only in four directions (horizontal and vertical), ensuring the path direction is completely consistent with the crop rows. Simultaneously, a turning penalty is introduced, incurring additional costs if the direction changes during node expansion, effectively suppressing frequent turns and encouraging long, straight-line travel. The resulting paths significantly reduce accidental trampling of soybean crops, aligning with the actual agronomical requirements of inter-row weeding, and improving path regularity, facilitating subsequent operations and trajectory tracking.

[0057] A rolling replanning mechanism based on forward-looking distance is introduced, combined with a path smoothing strategy that rounds corners at field edges and obstacles, achieving a balance between dynamic environmental adaptability and kinematic feasibility. When the robot executes its path, dynamic obstacles (including those of another robot and pre-set moving obstacles) are detected in real time. If the path ahead is encroached upon, replanning is triggered through the forward-looking window. A waiting mechanism after multiple failed replanning attempts and a target point failure mechanism prevent infinite loops. Simultaneously, post-processing of the path rounds corners near field edges and obstacles with minimum turning radius. While satisfying the kinematic constraints of agricultural machinery, this maintains the regularity of straight-line travel within the field, ensuring both operational efficiency and smooth, trackable path, significantly improving the algorithm's robustness and practicality in complex dynamic environments.

[0058] The above-disclosed embodiments are merely preferred embodiments of the present invention. However, the embodiments of the present invention are not limited thereto, and any variations that can be conceived by those skilled in the art should fall within the protection scope of the present invention.

Claims

1. A method for zoning path planning in weeding operations, characterized in that, Includes the following steps: Construct a work environment map, which includes: field boundaries, work area, and static obstacles within the work area; perform a safety buffer expansion on the static obstacles, with the expansion distance being the sum of the robot radius and the safety margin; The work area is discretized into a grid map, a sequence of target points to be covered is generated by a snake sorting algorithm, and the sequence of target points is divided into multiple independent task partitions according to the crop row direction. The multiple independent task partitions are assigned to the corresponding robots, and the path is prohibited from crossing the partition boundary by the region constraint function. The grid-based path planning method plans a collision-free path for each robot from its current position to the next target point within its partition. The path planning method adopts a four-adjacent motion model, and node expansion is only allowed in four directions: horizontal and vertical. A turning penalty term is introduced, which increases the path cost when the direction of movement changes in two consecutive steps. During the process of the robot executing a collision-free path, the system detects in real time whether there are dynamic obstacles on the path ahead. The dynamic obstacles include the real-time positions of other robots and preset moving obstacles. If a collision risk is detected, a rolling replanning mechanism is triggered to re-search for feasible paths using the forward distance as a window. The subsequent planned paths are then updated with the feasible paths. The updated planned path is post-processed and smoothed, including removing collinear redundant points, straightening straight lines, and rounding corners with minimum turning radius only in the field boundary area and / or near obstacles, to generate the final executable path. The field boundary area is a strip-shaped area inside the boundary formed by offsetting a preset threshold into the work area based on the field boundary. The preset threshold is determined according to the robot's minimum turning radius and / or safety margin.

2. The method for zoning path planning in weeding operations according to claim 1, characterized in that, The number of independent task partitions is set to two, corresponding to the first robot and the second robot respectively. The snake sorting algorithm includes the following steps: According to the set crop row direction, the robot is divided into grid columns and moves back and forth along the crop row direction within each column. It also thins out the work area in the direction of row change according to the work width. The task partitioning method is as follows: the field is divided into two parts along the crop row direction. The first robot is responsible for the lower half of the field, and the second robot is responsible for the upper half of the field, ensuring that the path of each robot is strictly limited to its own partition.

3. The method for zoning path planning in weeding operations according to claim 1, characterized in that, The rolling replanning mechanism includes: Get the current time and update the list of dynamic obstacles; after planning a new path, advance along the path point by point and check whether each path point is occupied by dynamic obstacles; if occupied, stop execution and record the number of blocking times. When the number of blocking times exceeds the threshold, mark the current target point as unreachable; if not occupied, update the robot's current position and mark the grid within the coverage area as covered.

4. The method for zoning path planning in weeding operations according to claim 3, characterized in that, The logic for marking the current target point as unreachable is as follows: for a target grid, if multiple robots fail to reach it, or if a target grid is located within the robot's partition but the robot fails to plan it multiple times, then the grid is removed from the set to be covered.

5. The method for zoning path planning in weeding operations according to claim 1, characterized in that, During the minimum turning radius rounding process, an arc transition segment that satisfies the minimum turning radius constraint is constructed based on the turning direction of adjacent path segments, and discretized collision detection is performed on the arc transition segment and the line segment connected to the original path. When the detection result meets the safety constraints, the inflection point in the original polyline path is replaced by the arc transition segment; otherwise, the original polyline path is retained.

6. The method for zoning path planning in weeding operations according to claim 1, characterized in that, The safety margin is a configurable parameter used to increase the clearance between the robot and obstacles beyond its physical radius, with a value ranging from 0.2 to 0.8 meters.

7. The method for zoning path planning in weeding operations according to claim 2, characterized in that, The first and second robots adopt an alternating advancement strategy. In each iteration, the robot that is closer to its current target point is selected first for path planning and execution in order to balance the progress of the two robots.

8. The method for zoning path planning in weeding operations according to claim 1, characterized in that, During the robot's execution along the final executable path, a circular area centered on the current execution path point and with a radius equal to half the width of the spray pattern is defined as the coverage neighborhood, and grids that fall into the coverage neighborhood and are located in the set to be covered are marked as covered.

9. The method for zoning path planning in weeding operations according to claim 1, characterized in that, The selection of the next target point adopts a combination of sequential priority and nearest point supplementation, including the following steps: priority is given to selecting the target points in the current partition according to the serpentine sorted target sequence; when the target sequence of the current partition is exhausted, the target point closest to the current robot position is selected from the uncovered set of the current partition as the next task target.

Images