An agricultural machine path planning method with full-process dynamic obstacle avoidance

Abstract

This invention proposes a full-process dynamic obstacle avoidance agricultural machinery path planning method, including the following steps: Step 1, creating a map outline for agricultural machinery operation; Step 2, generating a preliminary operation path based on the map outline and agricultural machinery parameters; Step 3, performing multi-constraint adaptive U-turn curve planning in the preliminary operation path to form a continuous drivable centerline; Step 4, checking the continuous drivable centerline and correcting it according to the motion curvature consistency requirement to obtain a corrected preliminary operation path; Step 5, monitoring obstacles in front of the agricultural machinery in real time and replanning the path based on the obstacles; Step 6, smoothing the replanned path and recalculating the agricultural machinery speed to complete the full-process dynamic obstacle avoidance agricultural machinery path planning.

Description

Technical Field

[0001] This invention relates to a method for agricultural machinery path planning, and more particularly to a method for agricultural machinery path planning with dynamic obstacle avoidance throughout the entire process. Background Technology

[0002] With the development of precision agriculture, the long-ridge operation in contiguous fields covering thousands of acres places dual demands on the automatic navigation systems of agricultural machinery, requiring both centimeter-level lateral accuracy and consistent operational quality throughout the process. Currently, mainstream technologies rely on "GNSS-RTK + inertial navigation" for absolute positioning, using simple AB straight lines or fixed curvature paths for tracking and control. However, these methods expose three core flaws in complex farmland scenarios: irregular field boundaries, recessed angles, and static obstacles lead to missed sowing and reseeding in traditional AB line planning; the lack of effective compensation mechanisms for machine attitude deviations caused by ground undulations affects operational accuracy; fixed-radius turning modes fail to consider the width of the field edges and soil conditions in different farmlands, resulting in increased ineffective travel distance, soil structure damage, and decreased fuel efficiency; and existing obstacle avoidance algorithms neglect ridge direction constraints and protection of unoperated areas during planning, leading to agronomic accidents such as seedling crushing and ridge trampling, damaging crop growth.

[0003] Current research, while attempting to introduce path optimization algorithms, still suffers from the following limitations: digital map construction relies on specialized surveying equipment, which is costly and difficult to reuse; U-turn curve planning is decoupled from agricultural machinery dynamics, making it prone to sideslip during turns; obstacle response strategies do not incorporate the differences in farmland conditions (paddy fields / dry fields / slopes), leading to a disconnect between obstacle avoidance behavior and agronomic requirements. Therefore, there is an urgent need for a full-process path planning method that integrates terrain adaptation, agronomically compliant obstacle avoidance, and multi-constraint U-turn optimization, which can solve the industry challenge of synergistic optimization of operational efficiency, soil protection, and crop safety while ensuring centimeter-level accuracy. Summary of the Invention

[0004] Purpose of the invention: The technical problem to be solved by the present invention is to provide a method for agricultural machinery path planning with dynamic obstacle avoidance throughout the entire process, which addresses the shortcomings of the existing technology.

[0005] To address the aforementioned technical problems, this invention discloses a method for agricultural machinery path planning with full-process dynamic obstacle avoidance, comprising the following steps:

[0006] Step 1: Create a map outline for agricultural machinery operations;

[0007] Step 2: Based on the map outline and agricultural machinery parameters, generate a preliminary operation path;

[0008] Step 3: In the initial work path, perform multi-constraint adaptive U-turn curve planning to form a continuous drivable centerline;

[0009] Step 4: Check the continuous drivable centerline and make corrections according to the requirements of motion curvature consistency to obtain the corrected preliminary work path;

[0010] Step 5: Monitor obstacles in front of the agricultural machinery in real time and replan the path based on the obstacles;

[0011] Step 6: Smooth the replanned path and recalculate the agricultural machinery speed to complete the agricultural machinery path planning for dynamic obstacle avoidance throughout the entire process.

[0012] Furthermore, the creation of the map outline for agricultural machinery operations described in step 1 includes:

[0013] The operation boundary and obstacles within the boundary were measured and calibrated manually to obtain the closed polygon. , serving as the map outline for agricultural machinery operations.

[0014] Furthermore, the generation of the preliminary job path in step 2 includes:

[0015] Step 2-1, based on the closed polygon Calculate the closed polygon using the direction of the land head as the reference. The minimum bounding rectangle (MBR);

[0016] Step 2-2: Determine the main working direction of the agricultural machinery. The details are as follows:

[0017] Define the longer side of the minimum bounding rectangle (MBR) as the main operating direction. If the closed polygon If there is a long row of rows within the work area, the main work direction should be manually corrected. ;

[0018] Steps 2-3: Set the row spacing for the agricultural machinery. and the operating width of agricultural machinery Calculate the actual working distance , means as follows:

[0019] ;

[0020] Steps 2-4: Generate the effective work area The details are as follows:

[0021] With polyhedrons Outward safety distance Shift inward to obtain the effective working area. ;

[0022] Steps 2-5, within the effective work area The initial job path is generated as follows:

[0023] Main work direction Based on this, within the effective working area According to the actual working spacing Generate a cluster of parallel line segments as the initial operation path, as shown below:

[0024] ;

[0025] in, Indicates the first There are 1 line segment, and the number of line segments is 1. .

[0026] Furthermore, the multi-constraint adaptive U-turn curve planning described in step 3, i.e., multi-constraint adaptive U-turn curve planning for the endpoints of the initial operation path, i.e., the parallel line segment cluster, includes:

[0027] Step 3-1, select the optimal U-turn mode, as shown below:

[0028] ;

[0029] in, The width of the land end. Soil moisture content, The minimum turning radius of the agricultural machinery is calculated as follows:

[0030] ;

[0031] in, For agricultural machinery wheelbase, This is the maximum steering angle of the agricultural machinery. The ground adhesion coefficient;

[0032] Step 3-2, calculate the length of the U-turn curve, as follows:

[0033] ;

[0034] in, Indicates the length of the U-turn curve;

[0035] Step 3-3, for each line segment symmetrical turning curves are generated at both ends. and Then check whether there is any interference with obstacles or the generated U-turn area; if there is interference, adjust the radius of the U-turn curve or insert a transition line until there is no interference.

[0036] Steps 3-4, cut the straight line segment With U-turn curve and By splicing together, a continuous, drivable centerline is formed. , for the center line Set the ID number, driving direction, and speed setting of the agricultural machinery.

[0037] Furthermore, the correction based on the consistency requirement of motion curvature described in step 4 includes:

[0038] Step 4-1, for each centerline segment Calculation speed , means as follows:

[0039] ;

[0040] in, Let be the radius of curvature. For the lateral acceleration of agricultural machinery, This is the maximum design speed in the current gear.

[0041] Step 4-2, if the velocity of any segment If the radius of the U-turn curve is lower than the minimum requirement for the operation, readjust the radius until the requirement is met;

[0042] Step 4-3, add all center lines Connect the routes according to the driving sequence to generate a revised preliminary work path. .

[0043] Furthermore, the path replanning based on obstacles described in step 5 includes:

[0044] Step 5-1: Detect obstacles and generate obstacle point clouds;

[0045] Step 5-2, Set a safe distance , means as follows:

[0046] ;

[0047] in, The real-time speed of agricultural machinery;

[0048] Step 5-3: Establish the Frenet coordinate system with the center of the rear axle of the current agricultural machinery as the origin. ,in, The longitudinal tangent of the axis along the original planned path is the same as the main working direction. parallel, Perpendicular to Axis points to closed polygon The inside;

[0049] Step 5-4, in Within the window, the unworked area is rasterized into a binary mesh with a preset resolution. , means as follows:

[0050] ;

[0051] In this context, the vertical boundary of the binary grid is the current node. and target node The horizontal boundaries are on the left and right sides of the original path. Place;

[0052] Step 5-5: The improved Hybrid-A* method is used to search the grid to obtain the replanned path; during the search, the node expansion cost function is calculated based on the unworked areas and the direction of the field ridges. , means as follows:

[0053] ;

[0054] in, This is the path length weighting coefficient. For steering constraint weighting coefficients, This represents the weighting coefficient for agricultural machinery. It is a turning radius penalty item. Weights for the direction of the field ridges. From the starting point to the current node The actual driving distance The heuristic distance is the distance to the current node. To the target The Euclidean distance;

[0055] When conducting a search, prioritize searching areas that are not currently in operation and... The nodes, among which, It is the minimum equivalent angle difference of the direction of movement of agricultural machinery from one node to the next.

[0056] Furthermore, the heuristic distance described in step 5-5 is calculated as follows:

[0057] ;

[0058] in, The target longitudinal position that the agricultural machinery is expected to reach along the reference path at the target time. This represents the current longitudinal position of the agricultural machinery along the reference path, i.e., the arc length coordinate. The desired lateral offset at the target time. This represents the vehicle's lateral offset relative to the reference path at the current moment.

[0059] Furthermore, the turning radius penalty item described in steps 5-5 This is used to ensure that the planned path meets the minimum turning radius requirements of agricultural machinery, and is represented as follows:

[0060] ;

[0061] in, This is the minimum turning radius of the agricultural machinery. This represents the current turning radius of the agricultural machinery. When the current turning radius is less than the minimum turning radius, Approaching infinity, the minimum turning radius constraint is forcibly satisfied, as shown below:

[0062] .

[0063] Furthermore, the field ridge direction weights mentioned in steps 5-5 The calculation method is as follows:

[0064] ;

[0065] in, The heading angle of the current node. Main work direction Angle, Due to the angle difference, and These are variable parameters, set according to the type of farmland.

[0066] Furthermore, step 6, which involves smoothing the replanned path and recalculating the agricultural machinery speed, includes:

[0067] Step 6-1: Perform cubic quasi-uniform B-spline smoothing on the replanned path;

[0068] Step 6-2, use the speed calculated in step 4-1 Using the same method, calculate the smoothed velocity. :

[0069] Step 6-3: Overlay the dynamic obstacle prediction scheme to calculate the safe speed of agricultural machinery. The recalculated agricultural machinery speed is expressed as follows:

[0070] ;

[0071] in, The dynamic safe speed limit, a speed threshold to ensure that agricultural machinery can safely stop or avoid obstacles, is calculated as follows:

[0072] ;

[0073] in, The relative distance between the agricultural machinery and the obstacle. The minimum deceleration required to brake urgently from the current speed to a safe state.

[0074] Beneficial effects:

[0075] 1. This invention achieves centimeter-level accuracy in dynamic path planning through a technical closed loop of three-dimensional map construction (step 1), multi-constraint turn-around optimization (step 3), and ridge-direction adaptive replanning (step 5), which significantly improves the operating accuracy and coverage of tractors in complex farmland environments.

[0076] 2. By taking into account the turning radius and dynamic model of agricultural machinery, this invention can generate an operation path that conforms to the physical constraints of agricultural machinery, and avoid the loss of operation efficiency caused by fixed turning methods in traditional path planning through the optimization of turning curves.

[0077] 3. This invention uses lidar and cameras to monitor dynamic obstacles in real time, and automatically triggers path replanning when obstacles are detected. This ensures that agricultural machinery can consider the direction of the farmland and field ridges and the type of farmland when safely avoiding obstacles during operation, thus avoiding interruptions, collisions and repeated operations.

[0078] 4. By smoothing the planned path and combining it with a dynamic obstacle prediction model, this invention can dynamically adjust the speed and path while ensuring path smoothness, thereby further improving the safety and stability of the operation.

[0079] 5. The job paths generated by this invention conform to a standardized format and can be reused as needed, eliminating the need for repeated calculations within a job season, thus reducing the computational burden on the system and improving job efficiency. Attached Figure Description

[0080] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. and / or Other advantages will become clearer.

[0081] Figure 1 This is a schematic diagram of the overall process of the present invention.

[0082] Figure 2 Create a schematic diagram for the initial operation path.

[0083] Figure 3a This is a schematic diagram of a circular 180° turn.

[0084] Figure 3b This is a schematic diagram of an Ω-shaped three-circular-arc U-turn. Detailed Implementation

[0085] This invention provides a full-process dynamic obstacle avoidance agricultural machinery path planning method. It aims to overcome the collaborative optimization challenges of "geometric accuracy, operational efficiency, agronomic compliance, and soil protection" through full-process technological reconstruction, providing centimeter-level reliable operational paths for unmanned farms covering tens of thousands of acres. In general, the key steps of this invention are as follows: digital map contour construction, preliminary operational path generation, multi-constraint adaptive U-turn curve planning, path kinematic consistency verification and correction, real-time dynamic obstacle detection and path replanning using an improved Hybrid-A* algorithm, path smoothing, and speed adjustment.

[0086] This invention According to By combining changes in farmland topography with real-time elevation data and dynamic obstacle information, precise path planning can be achieved, thereby significantly improving the efficiency and accuracy of tractor operations in complex farmland environments, enhancing operational safety, reducing crop damage during operations, and mitigating soil compaction caused by the work. The specific approach is as follows:

[0087] (1) In this invention, farmland boundaries are continuously recorded in advance using a centimeter-level RTK handheld terminal (GNSS+IMU), and Kalman filtering and smoothing techniques are applied to generate high-precision three-dimensional boundary point cloud data. The point cloud is thinned based on the Douglas-Puk algorithm, and a closed polygon boundary is generated using an improved convex hull algorithm.

[0088] This invention provides a stable centimeter-level spatial reference for path planning through a high-precision three-dimensional map outline, solving the problem of inaccurate boundary definition in traditional path planning; the boundary adaptability is enhanced, especially suitable for irregular fields, improving the reliability and accuracy of path planning.

[0089] (2) This invention establishes a multi-constraint adaptive U-turn decision model, combines the dynamic model of agricultural machinery with the minimum turning radius, and plans the U-turn curve. Different U-turn strategies (such as circular U-turn and Ω-shaped three-circle U-turn) are automatically switched according to different field widths and soil conditions to optimize path flexibility.

[0090] Traditional agricultural machinery path planning uses fixed-radius U-turns (circular or Ω-shaped), which results in low turning efficiency and severe soil compaction. This invention can intelligently select the most suitable U-turn method, avoiding the ineffective travel time caused by unreasonable U-turn design in traditional path planning, reducing ineffective travel distance at the field ends, preventing soil compaction, and reducing fuel consumption.

[0091] (3) The present invention monitors dynamic obstacles within a range of 60 meters ahead in real time using sensors such as lidar and cameras, and automatically triggers path replanning when an obstacle is detected, so as to ensure that the agricultural machinery can avoid the obstacle.

[0092] This invention detects dynamic obstacles in real time and replans the path based on the obstacle's location, greatly reducing the risk of work interruption or collision caused by obstacles; it ensures the safety of agricultural machinery, especially in complex scenarios (such as farmland with many dynamic obstacles), and avoids losses and wasted time due to obstacle interference during operation.

[0093] (4) This invention designs a Hybrid-A* replanning algorithm with ridge-direction constraints. General agricultural machinery replanning only considers obstacle avoidance and accessibility, which leads to agricultural machinery crushing seedlings and damaging field ridges. In local path planning, the situation of unoperated farmland is considered, a ridge directional weight function is designed, and an adaptive working condition model is developed.

[0094] This invention reduces the time spent on replanning, reduces the seedling crush rate, and ensures the consistency of crop row spacing.

[0095] The technical solution of the present invention, such as Figure 1 As shown, it includes the following steps:

[0096] Step 1: Create a map outline for agricultural machinery operations. Before the first entry of an unmanned farm or the start of the operating season, a reusable digital map outline containing three-dimensional boundaries, elevations, and static obstacles is created in a one-time, low-cost, and high-precision manner, providing a centimeter-level spatial reference for subsequent dynamic path planning.

[0097] The specific method for step 1 is as follows:

[0098] A centimeter-level RTK handheld terminal (with built-in multi-frequency, multi-satellite GNSS+IMU, supporting network RTK or local base station) was selected as the boundary acquisition device. Walking or driving a slow-moving vehicle along static obstacles such as the outer edge of the field, indented corners, ditches, utility poles, trees, and pumping stations, the WGS-84 coordinates and ellipsoidal height calculated by RTK were continuously recorded at a frequency of 1 Hz to 5 Hz. The acquired raw trajectory points were smoothed using Kalman filtering to remove jumps and multipath errors, obtaining an ordered boundary point cloud with a planar accuracy ≤2 cm and an elevation accuracy ≤3 cm. The point cloud was then thinned using the Douglas-Puk algorithm, achieving a compression rate ≥80% while ensuring a boundary geometric error <5 cm. An improved convex hull algorithm was applied to the compressed boundary to generate a closed polygon. .

[0099] Step 2: Based on the row spacing, map outline, turning radius of agricultural machinery, and dynamic model of tractor, generate preliminary operation path (straight part of agricultural machinery operation trajectory) to ensure that the path design meets the operation requirements.

[0100] The specific method for step 2 is as follows:

[0101] Read the closed polygon obtained in step 1 Using the direction of the field as a reference, the minimum bounding rectangle (MBR) is automatically calculated, and the longer side of the MBR is defined as the main working direction. If the farmland has a clear long-ridge orientation, manual correction is permitted. Ensure the main operating direction and ridge direction are consistent. Input the operating row spacing according to agronomic requirements. (e.g., sowing row spacing of 0.25 m, fertilization row spacing of 0.50 m) and width of agricultural machinery. Calculate the actual working distance This ensures that the row spacing is compatible with the physical constraints of the machine. (Using polygons) Outward safety distance (Default 0.5 m, to prevent equipment from colliding with the boundary) Offset inward to generate an effective working area. Subsequently Based on, in Inner spacing Generate a family of parallel lines The rendering is as follows Figure 2 As shown.

[0102] Step 3: Based on the tractor's dynamic model and minimum turning radius, plan a multi-constraint adaptive U-turn curve and optimize the path to form a continuous drivable centerline. Assign a unique ID, driving direction (forward / reverse), and speed level to ensure that the path is collision-free and maintains continuity, thereby meeting the requirements for flexibility and accuracy during operation.

[0103] The specific method for step 3 is as follows:

[0104] Based on the minimum turning radius of the tractor (Determined by vehicle wheelbase, maximum steering angle, and ground adhesion coefficient) and dynamic model, as well as real-time collected headway width. 、 Soil moisture content (Acquired via an onboard humidity sensor), perform multi-constraint adaptive U-turn curve planning on the endpoints of the straight line cluster, such as... Figure 3a and Figure 3b As shown, the optimal U-turn mode is:

[0105]

[0106] in From the wheelbase of agricultural machinery Maximum steering angle and ground adhesion coefficient Dynamic calculation:

[0107]

[0108] The formula for calculating the curve length in agricultural machinery operations is:

[0109] .

[0110] For each straight line segment symmetrical turning curves are generated at both ends. And check whether there is interference with obstacles or the generated U-turn area; if there is interference, adjust the radius of the arc through secondary optimization or insert a transition line until the collision-free constraint is met; then adjust the line segment. With U-turn curve By splicing together, a continuous drivable centerline is formed. It is assigned a unique ID, driving direction (forward / reverse), and speed level.

[0111] Step 4: Based on the agricultural machinery kinematics model, check whether the planned path meets the requirement of consistent motion curvature. If not, make corrections and connect the optimized paths into a complete operation path, outputting a standardized path format for subsequent use and reuse.

[0112] The specific method for step 4 is as follows:

[0113] Based on the tractor dynamics model Perform velocity-curvature consistency verification:

[0114]

[0115] longitudinal acceleration This is the maximum allowable longitudinal acceleration, which is the acceleration along the direction of travel and is related to the driving force. It is the gravitational acceleration constant. The soil adhesion coefficient limits the driving force. If the tractor is accelerating or climbing, insufficient longitudinal force may cause slippage or power interruption. In this case, speed must be limited to avoid overloading the drive system. Lateral acceleration. The maximum permissible lateral acceleration and the maximum design speed in the current gear. , The radius of curvature is used; the smaller the radius of curvature, the sharper the curve. If the speed in any segment is lower than the minimum operating requirement, the radius of the U-turn curve must be re-optimized; otherwise, it may lead to skidding or rollover. All centerlines... Connect according to the driving sequence to generate a preliminary work path. Output in a standard format (ISO 11783-7 TrackedPath or custom JSON), and synchronously include: the 3D coordinate sequence (x, y, z) of each path segment, curvature, heading, desired speed, equipment status (on / off), geometric parameters of the turning area, and soil compaction index. The data is sent to the agricultural machinery's onboard controller for local obstacle avoidance and high-precision tracking control; if the terrain or boundaries remain unchanged during the operating season... It can be called repeatedly without recalculation.

[0116] Step 5: Use lidar and cameras to monitor dynamic obstacles ahead in real time. If an obstacle is detected, establish a rasterized environmental map based on the Frencect coordinate system and automatically trigger the improved Hybrid-A* algorithm for path replanning. Taking into account the already operated area and the direction of the field ridge, ensure that the agricultural machinery can safely avoid obstacles and avoid operation interruption or collision.

[0117] The specific method for step 5 is as follows:

[0118] Using a 16-line LiDAR (120° field of view) and a wide-angle camera (2 megapixels), dynamic obstacles (farmers, animals) within a 60m fan-shaped area ahead are detected at a frequency of 20Hz. New obstacles (fallen crops, landslide mounds) are also detected. When the distance between the obstacle point cloud and the planned path in step two is less than the safe distance... When this occurs, replanning is triggered. The safe distance is affected by the speed of the agricultural machinery. When the speed of the agricultural machinery is greater than 2m / s, When the speed is less than 2m / s, The dynamic calculation model for safe distance is as follows:

[0119]

[0120] in This represents the real-time speed of the agricultural machinery. A Frenet coordinate system is established with the current rear axle center of the machinery as the origin. The longitudinal tangent direction along the original planned path (which is the same as the main operation direction in step 2) parallel), Perpendicular to

[0121] The axis points to the inside of the farmland; in Within the window, the unworked area is rasterized into a binary mesh G with a resolution of 0.1 m:

[0122]

[0123] The vertical boundary of the grid is and The lateral boundaries are 3 meters to the left and right of the original path to ensure that local maneuvers do not cross the effective agronomic area.

[0124] Using an improved Hybrid-A* algorithm in The search is performed within the grid, taking into account untouched areas and field ridge directions. The node expansion cost function is as follows:

[0125]

[0126] in This represents the actual distance traveled from the starting point to the current node. For heuristic distance (to target) The Euclidean distance (the distance between the two points) is expressed as:

[0127]

[0128] This is a path length weighting coefficient used to balance path smoothness and obstacle avoidance efficiency. Only nodes are allowed to fall into the path during the search process. The grid ensures the principle of "prioritizing untouched farmland". If there is no feasible and collision-free path within a 50m window, the algorithm automatically expands the window to 100m until a feasible path is found; if there is still no solution, a "stop and wait" command is triggered.

[0129] The steering constraint weighting coefficient forces the minimum turning radius of agricultural machinery to be met; the larger the value, the more it discourages sharp turns.

[0130] This is the agronomic direction weighting coefficient, which penalizes paths that deviate from the ridge direction. The larger the value, the stricter the penalty for traveling along the field ridge.

[0131] Turning radius penalty When the current radius is less than the minimum radius, Approaching infinity, and forcibly satisfying the minimum turning radius constraint, the model is as follows:

[0132]

[0133] Field ridge direction weight The calculation formula is as follows:

[0134]

[0135] The heading angle of the current node. Main work direction (Defined in step 2). The parameters are adaptively adjusted based on the detected farmland type, as shown in Table 1:

[0136] Table 1 Parameter Table

[0137]

[0138] During path search, priority is given to searching unworked areas and... The nodes (monotropic alignment priority).

[0139] Step 6: Smooth the waypoints output by Hybrid-A* and recalculate the speed in conjunction with the dynamic obstacle prediction model to ensure a smooth path and safe obstacle avoidance.

[0140] The specific method for step 6 is as follows: Perform cubic quasi-uniform B-spline smoothing on the discrete track points output by Hybrid-A*, ensuring continuous curvature and satisfying... For step 2 The formula recalculates the velocity point by point on the smoothed trajectory, but a dynamic obstacle prediction model needs to be superimposed on it:

[0141]

[0142] in, , This is the predicted relative distance to the obstacle. If the obstacle is stationary or far away, Automatically restore to .

[0143] Example:

[0144] This invention provides a dynamic obstacle avoidance method for agricultural machinery path planning throughout the entire process. By comprehensively applying high-precision positioning technology, dynamic path planning algorithms, and real-time obstacle detection and avoidance technology, it achieves precise operation path planning for tractors in complex farmland environments. The following detailed description of the unmanned farm machinery dynamic path planning method of this invention is illustrated with a specific embodiment. This embodiment selects a 100-mu (approximately 6.7 hectares) farmland for testing, using a Dongfanghong 1804 tractor. This farmland includes static obstacles such as long-ridge operation areas, recessed corners, ditches, trees, and pumping stations. The farmland boundaries are irregular, exhibiting a typical long-ridge operation environment. The specific requirements for farmland operation are: sowing operations with a row spacing of 0.25 meters, a tool width of 2 meters, and the presence of different types of obstacles and terrain variations within the operation area.

[0145] Work area settings:

[0146] Total farmland area: 100 mu

[0147] Long-ridge operation area: 1.5 meters wide, 0.25 meters between ridges.

[0148] Static obstacles: There is a ditch in the northeast corner, trees and a pump house in the southwest, and an indentation on the west side of the farmland.

[0149] 2. Equipment Configuration

[0150] This embodiment uses the Dongfanghong 1804 tractor as the working machinery, equipped with a centimeter-level precision RTK positioning system, lidar, camera and inertial navigation system (IMU) to realize functions such as path planning, obstacle detection and obstacle avoidance.

[0151] GNSS equipment: It adopts an RTK handheld terminal with an accuracy of 2 cm (supports multi-frequency multi-satellite GNSS+IMU), operates at a frequency of 1 Hz, and supports network RTK or local base station.

[0152] LiDAR: Equipped with a LiDAR (Velodyne VLP-16), operating at a frequency of 20Hz, with a detection range of 60 meters and an accuracy of 2 cm, mainly used for dynamic obstacle detection.

[0153] Camera: High-resolution RGB-D camera (Kinect V2) for identifying and locating static and dynamic obstacles, with a shooting frequency of 20Hz.

[0154] Agricultural machinery dynamics model: The dynamics model of the Dongfanghong tractor 1804 includes a wheelbase of 2.4 meters, a maximum steering angle of 35 degrees, and a minimum turning radius of 5.5 meters.

[0155] Path planning and optimization

[0156] Based on the 3D map of the farmland, the row spacing, the turning radius of the agricultural machinery, and the dynamic model, a preliminary operation path is generated.

[0157] Step 1: Map Outline Generation

[0158] The RTK handheld terminal was used to collect data on farmland boundaries and static obstacles. The agricultural machinery traveled along the outer edge of the field, ditches, trees and other obstacles, and collected WGS-84 coordinate and elevation data at a frequency of 1 Hz.

[0159] Kalman filtering was applied to the collected original trajectory points to remove multipath errors and jump data, resulting in an ordered boundary point cloud with a plane accuracy of ≤2 cm and an elevation accuracy of ≤3 cm.

[0160] The boundary point cloud is thinned using the Douglas-Puk algorithm, achieving a compression rate of over 80% while ensuring that the boundary geometric error is less than 5 cm, thus generating a closed polygon P0.

[0161] Step 2: Job Path Generation

[0162] Calculate the minimum bounding rectangle (MBR) from the closed polygon P0 generated in step 1, and define the main working direction L0.

[0163] Based on agronomic requirements (sowing row spacing d = 0.25 meters) and tractor implement width W = 2 meters, the actual working spacing is calculated as D = max(d, W) = 2 meters.

[0164] Within the farmland boundary, generate a cluster of parallel straight lines L1, L2, ..., L... at intervals D. N And based on the minimum turning radius Rmin (5.5 meters) and dynamic model to plan the turning curve at the endpoint of the straight line cluster.

[0165] Step 3: U-turn curve planning

[0166] Based on the tractor dynamics model, a pear-shaped 180° turn (two tangent circular arcs + a straight line segment) is adopted. If the working area is narrow, the turn is switched to an Ω-shaped three-circular arc or a reverse turn to ensure no sideslip and minimal ground compaction.

[0167] For each straight line segment L i A symmetrical turning curve C is generated at both ends. i start C i end Then check for interference with obstacles or U-turn areas. If interference exists, adjust the radius of the arc or insert transition lines through secondary optimization until the collision-free constraint is met.

[0168] 4. Dynamic obstacle detection and avoidance

[0169] Dynamic obstacle detection is performed using LiDAR and cameras. If an obstacle is detected, path replanning is automatically triggered.

[0170] Step 4: Obstacle Detection and Replanning

[0171] Use LiDAR and cameras to monitor dynamic obstacles (e.g., farmers, animals, etc.) in real time and trigger path replanning when an obstacle is detected.

[0172] When the distance between the obstacle and the planned path is less than the safe distance (d safe =1.5 meters, if the agricultural machinery speed is less than 2 m / s, d safe = 1.0 meter), the system will automatically perform path replanning to ensure operational safety.

[0173] In the Frenet coordinate system, the grid is binarized with a resolution of 0.1 meters, and the improved Hybrid-A* algorithm is used for local path planning to ensure the smoothness of the path and the obstacle avoidance effect.

[0174] 5. Path smoothing and speed adjustment

[0175] B-spline smoothing ensures continuous path curvature, and a dynamic obstacle prediction model adjusts the path speed.

[0176] Step 5: Path Smoothing and Speed ​​Adjustment

[0177] The discrete waypoints output by Hybrid-A* are smoothed using cubic quasi-uniform B-spline smoothing to ensure the curvature continuity of the path.

[0178] The system recalculates the speed point by point on the smoothed path and adjusts it in conjunction with a dynamic obstacle prediction model. If the obstacle is stationary or far away, the system restores the original path; if the obstacle is close, the path is adjusted appropriately to ensure operational safety.

[0179] In its specific implementation, this application provides a computer storage medium and a corresponding data processing unit. The computer storage medium is capable of storing a computer program, which, when executed by the data processing unit, can run the invention's content regarding a full-process dynamic obstacle avoidance agricultural machinery path planning method, as well as some or all of the steps in various embodiments. The storage medium can be a magnetic disk, optical disk, read-only memory (ROM), or random access memory (RAM), etc.

[0180] Those skilled in the art will clearly understand that the technical solutions in the embodiments of the present invention can be implemented using computer programs and their corresponding general-purpose hardware platforms. Based on this understanding, the technical solutions in the embodiments of the present invention, or the parts that contribute to the prior art, can be embodied in the form of computer programs, i.e., software products. These computer program software products can be stored in a storage medium and include several instructions to cause a device containing a data processing unit (which may be a personal computer, server, microcontroller, MCU, or network device, etc.) to execute the methods described in various embodiments or certain parts of the embodiments of the present invention.

[0181] This invention provides a method and approach for dynamic obstacle avoidance path planning of agricultural machinery throughout the entire process. Many methods and approaches exist for implementing this technical solution; the above description is merely a preferred embodiment of the invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of this invention, and these improvements and modifications should also be considered within the scope of protection of this invention. All components not explicitly stated in this embodiment can be implemented using existing technologies.

Claims

1. A method for agricultural machinery path planning with dynamic obstacle avoidance throughout the entire process, characterized in that, Includes the following steps: Step 1: Create a map outline for agricultural machinery operations; Step 2: Based on the map outline and agricultural machinery parameters, generate a preliminary operation path; Step 3: In the initial work path, perform multi-constraint adaptive U-turn curve planning to form a continuous drivable centerline; Step 4: Check the continuous drivable centerline and make corrections according to the requirements of motion curvature consistency to obtain the corrected preliminary work path; Step 5: Monitor obstacles in front of the agricultural machinery in real time and replan the path based on the obstacles; Step 6: Smooth the replanned path and recalculate the agricultural machinery speed to complete the agricultural machinery path planning for dynamic obstacle avoidance throughout the entire process. Step 3, which involves performing multi-constraint adaptive U-turn curve planning, specifically multi-constraint adaptive U-turn curve planning for the endpoints of the initial work path (i.e., the cluster of parallel line segments), includes: Step 3-1, select the optimal U-turn mode, as shown below: ； in, The width of the land end. Soil moisture content, The minimum turning radius of the agricultural machinery is calculated as follows: ； in, For agricultural machinery wheelbase, This is the maximum steering angle of the agricultural machinery. The ground adhesion coefficient; Step 3-2, calculate the length of the U-turn curve, as follows: ； in, Indicates the length of the U-turn curve; Step 3-3, for each line segment symmetrical turning curves are generated at both ends. and Then check whether there is any interference with obstacles or the generated U-turn area; if there is interference, adjust the radius of the U-turn curve or insert a transition line until there is no interference. Steps 3-4, cut the straight line segment With U-turn curve and By splicing together, a continuous, drivable centerline is formed. , for the center line Set the ID number, driving direction, and speed setting of the agricultural machinery.

2. The agricultural machinery path planning method for dynamic obstacle avoidance throughout the entire process according to claim 1, characterized in that, Step 1, which describes creating a map outline for agricultural machinery operations, includes: The operation boundary and obstacles within the boundary were measured and calibrated manually to obtain the closed polygon. , serving as the map outline for agricultural machinery operations.

3. The agricultural machinery path planning method for dynamic obstacle avoidance throughout the entire process according to claim 2, characterized in that, The generation of the preliminary job path in step 2 includes: Step 2-1, based on the closed polygon Calculate the closed polygon using the direction of the land head as the reference. The minimum bounding rectangle (MBR); Step 2-2: Determine the main working direction of the agricultural machinery. The details are as follows: Define the longer side of the minimum bounding rectangle (MBR) as the main operating direction. If the closed polygon If there is a long row of rows within the work area, the main work direction should be manually corrected. ; Steps 2-3: Set the row spacing for the agricultural machinery. and the operating width of agricultural machinery Calculate the actual working distance , means as follows: ； Steps 2-4: Generate the effective work area The details are as follows: With polyhedrons Outward safety distance Shift inward to obtain the effective working area. ; Steps 2-5, within the effective work area The initial job path is generated as follows: Main work direction Based on this, within the effective working area According to the actual working spacing Generate a cluster of parallel line segments as the initial operation path, as shown below: ； in, Indicates the first There are 1 line segment, and the number of line segments is 1. .

4. The agricultural machinery path planning method for dynamic obstacle avoidance throughout the entire process according to claim 3, characterized in that, The correction based on the consistency requirement of motion curvature mentioned in step 4 includes: Step 4-1, for each centerline segment Calculation speed , means as follows: ； in, Let be the radius of curvature. For the lateral acceleration of agricultural machinery, This is the maximum design speed in the current gear. Step 4-2, if the velocity of any segment If the radius of the U-turn curve is lower than the minimum requirement for the operation, readjust the radius until the requirement is met; Step 4-3, add all center lines Connect the routes according to the driving sequence to generate a revised preliminary work path. .

5. The agricultural machinery path planning method for dynamic obstacle avoidance throughout the entire process according to claim 4, characterized in that, Step 5, which involves path replanning based on obstacles, includes: Step 5-1: Detect obstacles and generate obstacle point clouds; Step 5-2, Set a safe distance , means as follows: ； in, The real-time speed of agricultural machinery; Step 5-3: Establish the Frenet coordinate system with the center of the rear axle of the current agricultural machinery as the origin. ,in, The longitudinal tangent of the axis along the original planned path is the same as the main working direction. parallel, Perpendicular to Axis points to closed polygon The inside; Step 5-4, in Within the window, the unworked area is rasterized into a binary mesh with a preset resolution. , means as follows: ； In this context, the vertical boundary of the binary grid is the current node. and target node The horizontal boundaries are on the left and right sides of the original path. Place; Step 5-5: The improved Hybrid-A* method is used to search the grid to obtain the replanned path; during the search, the node expansion cost function is calculated based on the unworked areas and the direction of the field ridges. , means as follows: ； in, This is the path length weighting coefficient. For steering constraint weighting coefficients, This represents the weighting coefficient for agricultural machinery. It is a turning radius penalty item. Weights for the direction of the field ridges. From the starting point to the current node The actual driving distance The heuristic distance is the distance to the current node. To the target The Euclidean distance; When conducting a search, prioritize searching areas that are not currently in operation and... The nodes, among which, It is the minimum equivalent angle difference of the direction of movement of agricultural machinery from one node to the next.

6. The agricultural machinery path planning method for dynamic obstacle avoidance throughout the entire process according to claim 5, characterized in that, The heuristic distance described in step 5-5 is calculated as follows: ； in, The target longitudinal position that the agricultural machinery is expected to reach along the reference path at the target time. This represents the current longitudinal position of the agricultural machinery along the reference path, i.e., the arc length coordinate. The desired lateral offset at the target time. This represents the vehicle's lateral offset relative to the reference path at the current moment.

7. The agricultural machinery path planning method for dynamic obstacle avoidance throughout the entire process according to claim 6, characterized in that, The turning radius penalty item mentioned in step 5-5 This is used to ensure that the planned path meets the minimum turning radius requirements of agricultural machinery, and is represented as follows: ； in, This is the minimum turning radius of the agricultural machinery. This represents the current turning radius of the agricultural machinery. When the current turning radius is less than the minimum turning radius, Approaching infinity, the minimum turning radius constraint is forcibly satisfied, as shown below: 。 8. The agricultural machinery path planning method for dynamic obstacle avoidance throughout the entire process according to claim 7, characterized in that, The field ridge direction weights mentioned in step 5-5 The calculation method is as follows: ； in, The heading angle of the current node. Main work direction Angle, Due to the angle difference, and These are variable parameters, set according to the type of farmland.

9. The agricultural machinery path planning method for dynamic obstacle avoidance throughout the entire process according to claim 8, characterized in that, Step 6, which involves smoothing the replanned path and recalculating the agricultural machinery speed, includes: Step 6-1: Perform cubic quasi-uniform B-spline smoothing on the replanned path; Step 6-2, use the speed calculated in step 4-1 Using the same method, calculate the smoothed velocity. : Step 6-3: Overlay the dynamic obstacle prediction scheme to calculate the safe speed of agricultural machinery. The recalculated agricultural machinery speed is expressed as follows: ； in, The dynamic safe speed limit, a speed threshold to ensure that agricultural machinery can safely stop or avoid obstacles, is calculated as follows: ； in, The relative distance between the agricultural machinery and the obstacle. The minimum deceleration required to brake urgently from the current speed to a safe state.

Images