An orchard intelligent agricultural machine precision operation path dynamic programming method and system

Abstract

The application discloses a kind of orchard intelligent agricultural machine precision operation path dynamic programming method and system, it is related to orchard intelligent agricultural machine path planning technical field, first acquisition airborne differential positioning, laser radar point cloud, panoramic vision, agricultural machine real-time pose multi-source perception data, complete space-time synchronization calibration and noise filtering pretreatment, identify the fixed boundary of operation area, obstacle position and orchard row parameter, combine agricultural machine operation width, kinematics constraint parameter generates no-collision initial global operation path and issues;Agricultural machine is along path operation process, real-time collection dynamic obstacle, orchard crown width change and row width information, filter transient interference target after judging whether to trigger path reprogramming.The application effectively reduces the path reprogramming false trigger rate under the scene of complex illumination, branch and leaf interference in orchard, can dynamically adjust planning priority according to the real-time distance of agricultural machine and obstacle, and give consideration to traffic safety and operation coverage accuracy.

Description

Technical Field

[0001] This invention relates to the field of intelligent agricultural machinery path planning technology in orchards, and in particular to a method and system for dynamic planning of precision operation paths for intelligent agricultural machinery in orchards. Background Technology

[0002] Currently, most mainstream orchard agricultural machinery path planning schemes adopt a pre-generated global path mode. This scheme first constructs a static operation map by collecting data on orchard topography, row distribution, and fixed obstacles in advance, and generates a fixed global operation path according to the requirements of full coverage operation. The agricultural machinery travels along the preset path to complete the operation. This scheme has simple calculation logic and stable path coverage, and has been widely used in commercial agricultural machinery autonomous navigation products. However, it is only suitable for static operation scenarios and cannot respond to dynamic changes such as the dynamic growth of fruit tree canopies, temporary stacking of agricultural materials, and the passage of operators in the orchard. During the passage, problems such as scraping fruit trees and colliding with obstacles are easy to occur. After encountering obstacles, the machine stops directly and waits for manual intervention, resulting in poor operation continuity and low overall efficiency.

[0003] Another mainstream solution is the real-time local obstacle avoidance mode. This solution adds sensing devices such as lidar to the global path to detect obstacles around the path in real time and generate local obstacle avoidance paths. It can respond to sudden obstacles and reduce the frequency of manual intervention to a certain extent. However, it uses a fixed-weight cost function for path planning and cannot dynamically adjust the planning priority according to the scenario. Either it will deviate too much from the work area to avoid obstacles, resulting in a high rate of missed coverage, or it will ignore the safety distance to ensure the accuracy of the operation, which will easily cause collision risks. At the same time, it is easy to misjudge the interference such as falling branches, birds, and ground light spots as obstacles, triggering frequent path adjustments. There is also the problem of sudden curvature when the obstacle avoidance path connects with the original path, which can cause the agricultural machinery to lose steering control and slip.

[0004] Orchards with different cultivation models have significant variations in terrain, ridge spacing, and canopy width. Different operational scenarios, such as weeding, spraying, harvesting, and transportation, also have different requirements for path accuracy and safety distance. Existing solutions cannot meet the multi-objective needs of operational efficiency, traffic safety, and coverage accuracy. There is an urgent need for a path planning solution that can adapt to the complex and dynamic scenarios of orchards. Summary of the Invention

[0005] This invention proposes a dynamic planning method and system for precision operation paths of intelligent agricultural machinery in orchards, in order to solve the problems mentioned in the prior art, such as poor adaptability of existing orchard agricultural machinery path planning, high rate of false triggering of replanning, and high rate of missed operation coverage.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: a dynamic planning method for precision operation paths of intelligent agricultural machinery in orchards, comprising the following steps: The first step involves collecting multi-source heterogeneous sensing data from the orchard operation area. This data includes airborne differential positioning data with a sampling frequency of no less than 10Hz, lidar point cloud data with an output frequency of no less than 15Hz, panoramic visual image data with a frame rate of no less than 25fps, and real-time pose data of the agricultural machinery with a sampling frequency of no less than 50Hz. A unified operation coordinate system is established with the kinematic center of the rear axle of the agricultural machinery as the origin. The timestamps of all sensing data are aligned through GNSS hardware timing. Based on the pre-calibrated sensor extrinsic parameters, the spatial coordinate transformation of the multi-source data is completed. Outlier noise, motion blur frames, and reflective interference points in the data are filtered out. Fixed operation boundaries, fixed obstacle positions, and fruit tree planting row parameters within the operation area are identified. This eliminates the sensing errors caused by sensor spatiotemporal misalignment from the data source and avoids environmental modeling deviations. The second step involves combining the agricultural machinery's own operating constraints, such as operating width, minimum turning radius, maximum steering angular velocity, and braking deceleration, to divide continuous operating strips according to the requirement of full coverage without repetition or omission. The initial global operating path is generated by matching the ridge direction and avoiding collisions. During the planning process, fixed obstacles are automatically avoided, and the turning path is optimized to reduce invalid backtracking and empty mileage. The initial path is then sent to the agricultural machinery operation control terminal to minimize operating energy consumption and non-operating time. The third step involves real-time sensing of the positions of dynamic obstacles in front of and to the sides of the work path, the outward extension of the fruit tree canopy, and the real-time passage width of the ridges during the operation of the agricultural machinery along the initial path. The detected targets are tracked continuously for multiple frames, and invalid interference such as birds flying, falling leaves, and ground reflections are filtered out. Based on the preset risk judgment rules, it is identified whether the dynamic path adjustment threshold is triggered to avoid frequent path adjustments caused by false alarms and ensure the continuity of operation. Fourth, when the path adjustment triggering condition is met, the current real-time position and real-time heading of the agricultural machinery are used as the starting point for replanning. Within the local search range of the adaptive braking distance in front of the agricultural machinery, a multi-objective dynamic programming cost function is constructed that integrates the cost of operation loss, the cost of passage safety, and the cost of path smoothing. The local optimal replanning path is selected by combining the kinematic speed constraints and steering constraints of the agricultural machinery to avoid generating sharp turning paths that exceed the tracking capability of the agricultural machinery. The calculation time of a single replanning operation meets the requirements of real-time control response. The fifth step is to smooth and verify the compliance of the connection between the replanned path and the remaining global path, check for collision risks and curvature abrupt changes in the connection, mark the missed work areas caused by the replanning offset, and send the verified updated path to the operation control terminal to drive the agricultural machinery to complete the remaining area operation along the updated path. After the main operation is completed, a supplementary operation path is automatically generated to cover the missed areas until the operation of the entire area meets the standards.

[0007] Furthermore, during coordinate calibration and data preprocessing, the acquisition trigger signals of each sensor are synchronized through hardware trigger lines, and the timestamp deviation of different sensors is controlled within 1ms. The external parameter rotation matrix and translation vector of each sensor relative to the rear axle center of the agricultural machinery are obtained through on-site target calibration. All sensing data are uniformly converted to the working coordinate system with the initial working direction of the agricultural machinery as the positive X-axis, the horizontal right direction perpendicular to the working direction as the positive Y-axis, and the vertical upward direction from the ground as the positive Z-axis. In the point cloud preprocessing stage, a statistical filtering algorithm is used to remove outliers that are more than a threshold away from the main body of the point cloud. A random sampling consistency algorithm is used to fit the ground plane model and segment the ground points, fruit tree canopy points, and obstacle points. In the image preprocessing stage, a dark channel dehazing algorithm and an adaptive histogram equalization algorithm are used to improve the image clarity in scenes with tree shading, backlighting, and low light, providing a high-quality data source for subsequent environmental recognition and effectively reducing the impact of complex orchard lighting conditions on perception accuracy.

[0008] Furthermore, during the identification of fixed operation boundaries, fixed obstacle locations, and fruit tree planting ridge parameters, the segmented non-ground point cloud is subjected to high-level clustering and geometric feature matching to distinguish different types of fixed obstacles such as cylindrical tree trunks, rectangular power and communication poles, linear irrigation pipelines, and guardrails. Differentiated safety buffer distances are set for different obstacle types. The buffer distance for tree trunk obstacles is matched with the scraping safety gap between the outer edge of the agricultural machinery and the tree trunk. The buffer distance for pole-shaped and pipeline obstacles additionally considers the extension length of the vehicle-mounted operating equipment. The random sampling consensus algorithm is used to fit the identified tree trunk center coordinates to a parallel straight line to obtain the slope and intercept of the fruit tree ridge. The passable furrow area between adjacent ridges is extracted to construct a static passable grid map with a resolution of 5cm. Each grid is marked with three states: passable, obstacle, and unknown. The map is updated in real time as the agricultural machinery moves, thus avoiding path scraping of fruit trees or machinery collisions with fixed facilities from the modeling level.

[0009] Furthermore, during the initial global operation path generation process without collisions, an improved ox-plowing full-coverage algorithm adapted to the characteristics of orchard ridge cultivation is adopted. It prioritizes generating straight path segments along the direction of the furrows, and sets the overlap rate of the width of adjacent operation strips to 5% to 8% of the operation width, taking into account both the integrity of ridge covering and the control of repetitive operation rate. In the turning area at the edge of the field, U-shaped turning, fishtail turning, or back-turning mode is automatically matched according to the available space at the current edge of the field. The path curvature of all turning segments strictly corresponds to the minimum turning radius constraint of the agricultural machinery to avoid damage to the soil and fruit trees in the orchard by turning in place or making sharp turns at large angles. After the path is generated, collision detection is carried out point by point. Waypoints that are less than the safety threshold from obstacles are offset towards the passable area in a direction perpendicular to the path until the safety distance requirement is met. Finally, the total operation mileage, total number of turns, and expected operation energy consumption of the entire path are simulated and calculated. The path is iteratively optimized until the three indicators reach Pareto optimality, which greatly improves the operation efficiency and traffic safety of the initial path.

[0010] Furthermore, in the process of determining the dynamic adjustment threshold of the path, the point cloud clustering results and visual target detection results are integrated to identify four types of targets that affect passage: workers, mobile machinery, temporarily stacked fruit boxes and agricultural materials, and outstretched fruit tree branches. The Kalman filter algorithm is used to continuously track each detected target, predict the target's trajectory in the next 1 to 3 seconds, and calculate the nearest encounter distance and encounter time between the target and the current planned path. When the nearest encounter distance is less than half the safe passage width of the agricultural machinery and the encounter time is less than the time required for the agricultural machinery to decelerate from the current operating speed to zero, or when the real-time passable width in the furrow due to the extension of branches is less than the sum of the width of the agricultural machinery body and the safety gap on both sides, it is determined that the replanning trigger condition is met. For instantaneous interference targets with a continuous tracking time of less than 3 frames, replanning is not triggered, thereby reducing the operation interruption and path oscillation caused by invalid replanning from the triggering mechanism level.

[0011] Furthermore, the expression for calculating the total cost function of multi-objective dynamic programming is as follows: ;in, The total value of candidate path points. , , These are the adaptive weight coefficients corresponding to the operation loss cost, traffic safety cost, and path smoothing cost, respectively, and the sum of the three weight coefficients is 1; The cost of lost work is the cost of subsequent work in the missed work area, calculated based on the lateral offset of the candidate path point relative to the center line of the original work strip. The larger the offset, the higher the cost of lost work. The collision risk cost is calculated based on the minimum distance between the candidate path point and surrounding obstacles and fruit tree trunks, with the higher the safety cost being the closer the distance. The path smoothing cost is the agricultural machinery steering control cost calculated based on the rate of change of heading angle between three adjacent candidate path points. The greater the rate of change of heading angle, the higher the smoothing cost. The weighting coefficients are dynamically and continuously adjusted based on the real-time distance from the current position of the agricultural machinery to the nearest obstacle, avoiding the poor scene adaptability problem caused by fixed weights. The adjustment rules satisfy: ;in, This represents the real-time distance from the current position of the agricultural machinery to the nearest obstacle. This refers to the emergency braking distance of agricultural machinery when it is traveling at its current operating speed. The system sets a safe warning distance for agricultural machinery; the local path search adopts an improved dynamic window method, which generates continuous candidate paths within the feasible speed space and turning space of the agricultural machinery. After calculating the total cost value point by point, the path with the lowest total cost is selected as the replanning result. The calculation time of a single replanning is controlled within 100ms, so as to achieve a dynamic balance between safety risk and operation efficiency.

[0012] Furthermore, during the smooth connection verification process between the replanned path and the remaining global path, a fifth-order polynomial curve is used to fit the connection segment. During fitting, it is ensured that the position, heading angle, and curvature of the starting point of the connection segment are completely consistent with the position, heading angle, and curvature of the ending point of the replanned path, and that the position, heading angle, and curvature of the ending point of the connection segment are completely consistent with the access point of the remaining global path, achieving continuous curvature without abrupt changes, thus avoiding problems such as sudden steering and bumpy driving when the agricultural machinery switches paths. The turning radius of each point on the fitted curve is verified point by point to ensure that the turning radius is greater than the minimum turning radius of the agricultural machinery. At the same time, the distance from the path point to the surrounding obstacles is checked point by point to ensure that all points meet the safety buffer distance requirements. If the verification fails, the local path search range is expanded and the replanned path is regenerated until the verification conditions are met. After the verification passes, the coordinates of the uncovered areas caused by the offset of the original operation strip during the replanning process are stored in the missed operation mark queue. After the main operation path is fully traversed, the shortest supplementary operation path connecting all the missed operation points is automatically generated to achieve no missed operations in the entire area.

[0013] Furthermore, an intelligent agricultural machinery precision operation path dynamic planning system for orchards includes a multi-source data perception and synchronization module, a global path optimization and generation module, a dynamic target tracking and triggering module, a local path adaptive replanning module, a path smooth connection verification module, and an operation closed-loop tracking and control module deployed on an onboard high-performance edge computing unit. The multi-source data sensing and synchronization module connects to the airborne differential positioning receiver, 3D LiDAR, panoramic vision camera, inertial measurement unit and wheel speed sensor through a hardware interface. It achieves time synchronization of all sensors through GNSS time synchronization and hardware trigger line, completes coordinate transformation, noise filtering and quality enhancement of sensing data, and outputs a high-precision static environmental grid map of the work area. The global path optimization generation module has a built-in agricultural machinery parameter storage unit, which stores inherent parameters such as the working width, minimum turning radius, maximum steering angular velocity, and braking deceleration of the agricultural machinery. Based on the static environment grid map, it generates a collision-free initial global operation path that meets the full coverage requirements. It supports simulation and iterative optimization of path operation efficiency and safety, and sends the optimized initial path to the operation closed-loop tracking control module. The dynamic target tracking and triggering module processes the sensing data stream in real time, continuously tracks and predicts the movement trajectory of dynamic targets, outward fruit branches, and changes in ridge width within the work area, filters out invalid interference targets, triggers replanning signals based on risk judgment rules, and locks the current posture of the agricultural machinery in real time as the replanning benchmark. After receiving the replanning trigger signal, the local path adaptive replanning module searches for the locally optimal replanning path that satisfies the kinematic constraints of the agricultural machinery within the local search range of the adaptive braking distance in front of the agricultural machinery using a weighted adaptive multi-objective cost function. The path smooth connection verification module completes the curvature continuous fitting of the replanned path and the remaining global path, performs collision detection and curvature compliance verification point by point, marks the missing work area and generates the supplementary work path after the main work is completed, and sends the verified complete updated path to the work closed-loop tracking control module. The operation closed-loop tracking control module connects the steering actuator and drive actuator of the agricultural machinery. It uses a pre-aiming tracking algorithm to correct driving deviations in real time and drive the agricultural machinery to smoothly complete the operation in the whole area along the planned path.

[0014] Furthermore, the multi-source data sensing synchronization module has a built-in timing signal distribution unit to provide a unified microsecond-level time reference for all connected sensors, a built-in external parameter storage unit to store the rotation matrix and translation vector of each sensor relative to the rear axle center of the agricultural machinery obtained through on-site target calibration, a built-in point cloud processing subunit to realize point cloud noise filtering, ground segmentation and obstacle clustering functions, and a built-in image processing subunit to realize low-light image enhancement, motion blur frame removal and target detection functions. The module supports outputting a 5cm resolution access grid map, and the map update frequency is consistent with the LiDAR output frequency. It can adapt to the complex environment of orchards with tree shade, intertwined branches and leaves, and variable lighting, providing stable and reliable environmental perception results for path planning and avoiding path planning errors caused by perception bias.

[0015] Furthermore, the operation closed-loop tracking control module has a built-in predictive distance adaptive adjustment unit, which dynamically adjusts the distance of the path prediction point according to the current driving speed of the agricultural machinery. The higher the driving speed, the farther the prediction distance, and the lower the driving speed, the closer the prediction distance. It also has a built-in deviation calculation unit, which collects the actual position, actual heading and actual steering angle of the agricultural machinery in real time, calculates the lateral deviation and heading deviation of the agricultural machinery relative to the planned path, and uses a pure tracking algorithm to output the front wheel steering angle control quantity and an incremental PID algorithm to output the driving speed control quantity. After the path replanning update is completed, the module automatically resets the pre-aiming point position and control parameters to avoid control oscillations and tracking deviations caused by the switching between the old and new paths. This forms a multi-closed-loop control of position, speed, and turning angle, ensuring that the agricultural machinery can track the planned path with high precision and reducing problems such as branch pressing, tree scraping, and missed covering during operation.

[0016] Compared with existing technologies, the beneficial effects of this invention are: This invention effectively eliminates perception errors in complex lighting and intertwined branches and leaves in orchards by using spatiotemporal synchronous calibration of multi-source sensing data and continuous multi-frame tracking and filtering of dynamic targets. It can accurately identify real passage risks and filter out instantaneous interference targets, and trigger path replanning only when there are actual passage obstacles. This effectively solves the problems of poor operation continuity and path travel oscillation caused by frequent false triggering of replanning in existing technologies.

[0017] This invention employs a multi-objective cost function with adaptive weight adjustment to perform local path replanning. It can dynamically adjust the priority of three types of costs—operation loss, traffic safety, and path smoothness—based on the real-time distance between the agricultural machinery and surrounding obstacles. When the distance to an obstacle is close, traffic safety is prioritized, while when the distance to an obstacle is far, operation coverage efficiency is prioritized. This effectively solves the problems of poor adaptability, risk of scraping, or ineffective detours in existing fixed-weight planning technologies.

[0018] This invention ensures the curvature of the path switching process is continuous and without abrupt changes by using the technical means of verifying the path connection with continuous curvature and automatically marking and supplementing the planning of missed work areas. This avoids problems such as agricultural machinery slipping and scraping fruit trees caused by sudden turning. At the same time, it automatically marks the missed plots caused by the deviation of the work area during the replanning process and automatically generates a supplementary work path after the main work is completed. This effectively solves the problems of abrupt path connection and high rate of missed work coverage in the existing technology. Attached Figure Description

[0019] Figure 1 This is the overall flowchart of the dynamic planning of precision operation path for intelligent agricultural machinery in orchards proposed in this invention; Figure 2 This is a flowchart of the multi-source sensing data preprocessing and static environment modeling process of the present invention; Figure 3 This is a flowchart of the collision-free initial global job path generation process of the present invention; Figure 4 This is a flowchart of the dynamic obstacle perception and replanning trigger judgment process of the present invention; Figure 5 This is a flowchart of the path replanning and smooth connection based on the dynamic cost function of this invention. Detailed Implementation

[0020] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0021] Reference Figures 1 to 5 This invention discloses a method and system for dynamic planning of precision operation paths for intelligent agricultural machinery in orchards. It includes a complete operational logic encompassing perception, planning, and control deployed on the intelligent agricultural machinery. The entire operation process is automatically completed in a closed loop, involving environmental perception, path generation, dynamic adjustment, and tracking control. Upon system startup, multi-source heterogeneous perception data from the orchard operation area is collected. All sensors establish an operational coordinate system with the rear axle kinematic center of the machinery as the origin. GNSS hardware time synchronization aligns the timestamps of all perception data. Based on pre-calibrated sensor extrinsic parameters, spatial coordinate transformation of the multi-source data is performed. Outlier noise, motion-blurred frames, and reflective interference points in the collected data are automatically filtered out. Fixed operation boundaries, fixed obstacle locations, and fruit tree planting row parameters within the operation area are identified. This eliminates perception errors caused by sensor spatiotemporal misalignment from the data source, avoiding environmental modeling biases.

[0022] After completing the environmental modeling, the system combines the agricultural machinery's own operating width, minimum turning radius, maximum steering angular velocity, braking deceleration and other operating constraints, divides continuous operating strips according to the full coverage requirement of no repetition and no omission, matches the row direction to generate a collision-free initial global operating path, automatically avoids fixed obstacles during the planning process, optimizes turning paths to reduce invalid backtracking and empty driving mileage, and sends the initial path to the agricultural machinery operation control terminal to minimize operating energy consumption and non-operating time.

[0023] During the operation of agricultural machinery along the initial path, the system senses the position of dynamic obstacles in front of and to the side of the operation path, the outward extension of the fruit tree canopy and the real-time passage width of the ridges in real time. It continuously tracks the detected targets for multiple frames, filters out invalid interference such as birds flying, falling branches and leaves, and ground reflections that appear instantaneously, and identifies whether the path dynamic adjustment threshold is triggered based on preset risk judgment rules, so as to avoid frequent path adjustments caused by false alarms and ensure the continuity of operation.

[0024] When the path adjustment trigger condition is met, the system takes the current real-time position and real-time heading of the agricultural machinery as the starting point for replanning. Within the local search range of the adaptive braking distance in front of the agricultural machinery, it constructs a multi-objective dynamic programming cost function that integrates the cost of operation loss, the cost of passage safety, and the cost of path smoothing. Combining the kinematic speed constraints and steering constraints of the agricultural machinery, it selects the locally optimal replanning path to avoid generating sharp turning paths that exceed the tracking capability of the agricultural machinery. The calculation time of a single replanning operation meets the requirements of real-time control response.

[0025] After completing the local path replanning, the system smooths and verifies the compliance of the connection between the replanned path and the remaining global path, checks for collision risks and curvature abrupt changes in the connection, marks the missed work areas caused by the replanning offset, and sends the verified updated path to the operation control terminal to drive the agricultural machinery to complete the remaining area operation along the updated path. After the main operation is completed, a supplementary operation path is automatically generated to cover the missed areas until the operation coverage of the entire area meets the standard.

[0026] This invention also discloses the specific implementation logic of coordinate calibration and data preprocessing. A hardware trigger line connects each sensor to the GNSS timing module, synchronizing the acquisition trigger signals of each sensor and controlling the timestamp deviation of different sensors to within 1ms. The system obtains the external parameter rotation matrix and translation vector of each sensor relative to the rear axle center of the agricultural machinery through on-site target calibration, and uniformly transforms all sensing data into an operational coordinate system with the initial working direction of the agricultural machinery as the positive X-axis, the horizontal rightward direction perpendicular to the working direction as the positive Y-axis, and the vertical upward direction from the ground as the positive Z-axis.

[0027] In the point cloud preprocessing stage, a statistical filtering algorithm is used to remove outliers that are more than a threshold away from the main body of the point cloud. When the algorithm is running, the average distance of the 30 neighboring points in the neighborhood of each point is calculated, and points whose average distance exceeds one standard deviation of the global average distance are removed. Then, a random sampling consistency algorithm is used to fit the ground plane model and segment the ground points, fruit tree canopy points and obstacle points.

[0028] The image preprocessing stage employs a dark channel dehazing algorithm and an adaptive histogram equalization algorithm. The sliding window size for dark channel dehazing is set to 15 pixels × 15 pixels, which effectively improves image clarity in scenes with tree shading, backlighting, and low light conditions, providing a high-quality data source for subsequent environmental recognition and effectively reducing the impact of complex orchard lighting conditions on perception accuracy.

[0029] This invention also discloses the specific identification logic for fixed operation boundaries, fixed obstacle positions, and fruit tree planting ridge parameters. It performs high-level clustering and geometric feature matching on the segmented non-ground point cloud to distinguish different types of fixed obstacles such as cylindrical tree trunks, rectangular power communication poles, linear irrigation pipelines, and guardrails. Differentiated safety buffer distances are set for different obstacle types. The buffer distance for tree trunk obstacles is matched with the scraping safety gap between the outer edge of the agricultural machinery and the tree trunk. The buffer distance for pole-shaped and pipeline obstacles additionally considers the extension length of the vehicle-mounted operating equipment.

[0030] The system employs a random sampling consensus algorithm to fit parallel lines to the identified tree trunk center coordinates. Each time, two tree trunk center coordinates are randomly selected to calculate the line parameters, iterating 1000 times to find the group of parallel lines with the most interior points. This yields the slope and intercept of the fruit tree ridge lines. The system extracts the passable furrow area between adjacent ridge lines. If the distance between adjacent ridge lines deviates from the standard ridge spacing by more than 20 cm, the fitting process is repeated to eliminate ridge line identification errors caused by individual tree trunk position offsets. The final environmental modeling output is a static passable grid map with a resolution of 5 cm. Each grid is marked with three states: passable, obstacle, and unknown. The map is updated in real-time using Bayesian estimation as the agricultural machinery moves. A grid occupancy probability exceeding 0.7 is marked as an obstacle, and below 0.3 is marked as passable. This modeling approach avoids path scraping against fruit trees or machinery colliding with fixed structures.

[0031] This invention also discloses the specific generation logic of the collision-free initial global operation path. It employs an improved ox-plowing-style full-coverage algorithm adapted to the characteristics of orchard ridge cultivation. It prioritizes generating straight path segments along the furrow direction, with waypoint intervals set to 1 meter. The overlap rate of adjacent operation strips is set to 5% to 8% of the operation width, balancing ridge coverage integrity with repetitive operation control. In the turning area at the edge of the field, the system automatically matches U-shaped turns, fishtail turns, or backtracking turns based on the available space at the current edge. The path curvature of all turning segments strictly corresponds to the minimum turning radius constraint of the agricultural machinery, avoiding damage to the soil and fruit tree roots from on-the-spot turns or sharp turns at large angles.

[0032] After the path is generated, collision detection is carried out point by point. The agricultural machinery is simplified into a rectangular envelope model with the same size as the actual size. The overlap between the agricultural machinery envelope and the obstacle grid at each waypoint is detected. For waypoints that are less than the safety threshold from the obstacle, they are offset in steps of 2 cm along the direction perpendicular to the path to the passable area until the safety distance requirement is met.

[0033] After the initial path is generated, the system automatically simulates and calculates three indicators for the entire path: total operating mileage, total number of turns, and estimated operating energy consumption. The Pareto optimization algorithm is used to iteratively optimize the path, with the number of iterations set to 20, to find the path scheme that achieves Pareto optimality for the three indicators, which greatly improves the operating efficiency and traffic safety of the initial path.

[0034] This invention also discloses the specific judgment logic of the path dynamic adjustment threshold. It integrates point cloud clustering results with visual target detection results to identify four types of targets affecting passage: workers, mobile machinery, temporarily stacked fruit boxes and agricultural materials, and outstretched fruit tree branches. The visual target detection model adopts the YOLOv8s network structure and is trained by collecting 12,000 orchard scene labeled images. TensorRT is used to accelerate low-latency inference.

[0035] The system employs a Kalman filter algorithm to continuously track each detected target. The state vector includes the target's planar coordinates and its velocity and acceleration in two orthogonal directions. A Hungarian algorithm is used to match targets between adjacent frames, predicting the target's trajectory for the next 1 to 3 seconds, and calculating the nearest encounter distance and time between the target and the currently planned path. The system calculates the emergency braking distance in real-time based on the agricultural machinery's current speed by dividing the square of the current speed by twice the braking deceleration. Replanning is triggered when the nearest encounter distance is less than half the safe passage width of the agricultural machinery, and the encounter time is less than the time required for the machinery to decelerate from its current operating speed to zero; or when the real-time passable width within the furrow due to branches extending outwards is less than the sum of the machinery's width and the safety clearance on both sides. For transient interference targets with a continuous tracking duration of less than 3 frames, the system classifies them as invalid targets such as birds or falling leaves, and does not trigger replanning. This reduces operational interruptions and path oscillations caused by invalid replanning at the triggering mechanism level.

[0036] This invention also discloses the specific calculation logic of the cost function for multi-objective dynamic programming, and the expression for calculating the total cost of candidate path points is as follows: ;in, The total value of candidate path points. , , These are the adaptive weight coefficients corresponding to the operation loss cost, traffic safety cost, and path smoothing cost, respectively, and the sum of the three weight coefficients is 1. The cost of work loss is the cost of subsequent work in the missed work area calculated based on the lateral offset of the candidate path point relative to the center line of the original work strip. The larger the offset, the higher the cost of work loss. During the calculation, the lateral offset is divided by the work width for normalization, and the value range is from 0 to 1. The collision risk cost is calculated based on the minimum distance between the candidate path point and surrounding obstacles and tree trunks. The closer the distance, the higher the safety cost. The calculation takes the reciprocal of the minimum distance for normalization, and the value ranges from 0 to 1. The path smoothing cost is the agricultural machinery steering control cost calculated based on the rate of change of heading angle between three adjacent candidate path points. The larger the rate of change of heading angle, the higher the smoothing cost. During the calculation, the rate of change of heading angle is divided by the maximum allowable rate of change of heading angle for normalization. The value range is from 0 to 1.

[0037] The weighting coefficients are dynamically and continuously adjusted based on the real-time distance from the current position of the agricultural machinery to the nearest obstacle, avoiding the poor scene adaptability problem caused by fixed weights. The adjustment rules satisfy: ;in, This represents the real-time distance from the current position of the agricultural machinery to the nearest obstacle. This refers to the emergency braking distance of agricultural machinery when it is traveling at its current operating speed. A pre-set safety warning distance is provided for agricultural machinery. Local path search employs an improved dynamic window method, generating continuous candidate paths within the feasible speed and turning space of the agricultural machinery. The linear velocity search range matches the typical driving speed range of orchard operations, while the angular velocity search range matches the maximum turning angular velocity of the machinery. After calculating the total cost at each point, the path with the lowest total cost is selected as the replanning result. The onboard edge computing unit uses a Jetson Orin NX core board, achieving a computing power of 100 TOPS, which can control the calculation time of a single replanning operation to within 100ms, achieving a dynamic balance between safety risks and operational efficiency.

[0038] This invention also discloses the specific implementation logic for the smooth connection verification between the replanned path and the remaining global path. A fifth-order polynomial curve is used to fit the connection segment. During the fitting, it is ensured that the position, heading angle, and curvature of the starting point of the connection segment are completely consistent with the position, heading angle, and curvature of the ending point of the replanned path, and that the position, heading angle, and curvature of the ending point of the connection segment are completely consistent with the access point of the remaining global path. This achieves continuous curvature without abrupt changes, avoiding the problem of sudden steering and bumpy driving when agricultural machinery switches paths.

[0039] Curvature calculation uses the plane curve curvature formula, which is the absolute value of the cross product of the first and second derivatives of the horizontal and vertical coordinates of the path point with respect to the arc length, divided by the cube of the first derivative modulus. The turning radius of each point on the fitted curve is checked point by point to ensure that the turning radius is greater than the minimum turning radius of the agricultural machinery. At the same time, the distance from the path point to the surrounding obstacles is checked point by point to ensure that all points meet the safety buffer distance requirements. If the check fails, the global path access point is moved back 1 meter to expand the local path search range and regenerate the replanned path until the check conditions are met.

[0040] After verification, the system stores the coordinates of uncovered areas that have deviated more than 10 centimeters from the original work strip during the replanning process into the missing work mark queue. After the main work path has been traversed, the traveling salesman algorithm is used to generate the shortest supplementary work path that connects all the missing work points, so as to achieve no missing work in the whole area.

[0041] This invention also discloses the overall architecture of a dynamic planning system for precision operation paths of intelligent agricultural machinery in orchards, implementing the above-mentioned method. All functional modules are deployed on an onboard high-performance edge computing unit, running on an Ubuntu 20.04 operating system. The ROS2 Humble robot middleware is used to achieve low-latency topic communication between modules, with communication latency below 10 milliseconds. The entire system includes a multi-source data perception synchronization module, a global path optimization generation module, a dynamic target tracking and triggering module, a local path adaptive replanning module, a path smooth connection verification module, and an operation closed-loop tracking control module. The multi-source data perception synchronization module connects to an onboard differential positioning receiver, a 3D LiDAR, a panoramic vision camera, an inertial measurement unit, and a wheel speed sensor via hardware interfaces. GNSS timing and hardware trigger lines achieve time synchronization of all sensors, completing coordinate transformation, noise filtering, and quality enhancement of the perceived data, and outputting a high-precision static environmental grid map of the operation area.

[0042] The global path optimization generation module has a built-in agricultural machinery parameter storage unit, storing inherent parameters such as the agricultural machinery's working width, minimum turning radius, maximum steering angular velocity, and braking deceleration. Based on a static environment grid map, it generates a collision-free initial global working path that meets full coverage requirements. It supports simulation and iterative optimization of path operation efficiency and safety, and sends the optimized initial path to the operation closed-loop tracking control module. The dynamic target tracking and triggering module processes the sensing data stream in real time, continuously tracks and predicts the movement trajectory of dynamic targets, outstretched fruit branches, and changes in ridge width within the working area, filters out invalid interference targets, triggers a replanning signal based on risk judgment rules, and locks the agricultural machinery's current posture as the replanning reference in real time.

[0043] Upon receiving a replanning trigger signal, the local path adaptive replanning module searches for a locally optimal replanning path that satisfies the kinematic constraints of the agricultural machinery within a local search range that accommodates the braking distance ahead of the machinery, using a weighted adaptive multi-objective cost function. The path smoothing and verification module performs continuous curvature fitting between the replanned path and the remaining global path, conducts collision detection and curvature compliance verification point by point, marks missed work areas, generates a supplementary work path after the main work is completed, and sends the verified complete updated path to the work closed-loop tracking control module.

[0044] The operation closed-loop tracking control module connects the steering actuator and drive actuator of the agricultural machinery. It uses a pre-aiming tracking algorithm to correct driving deviations in real time and drive the agricultural machinery to smoothly complete the operation in the whole area along the planned path.

[0045] This invention also discloses the hardware composition and processing logic of a multi-source data sensing synchronization module. The module has a built-in timing signal distribution unit that connects to the second pulse signal output by the differential positioning module, providing a unified microsecond-level time reference for all connected sensors. The module also has a built-in extrinsic parameter storage unit that stores the rotation matrix and translation vector of each sensor relative to the rear axle center of the agricultural machinery, obtained through on-site calibration using the AprilTag calibration board. The extrinsic parameters are solved using the Kalibr calibration tool, with the overall calibration error controlled within 2 cm. The module also has a built-in point cloud processing subunit that performs point cloud noise filtering, ground segmentation, and obstacle clustering based on a point cloud library. Finally, the module has a built-in image processing subunit that uses the OpenCV computer vision library to perform low-light image enhancement and motion-blurred frame removal, and deploys an accelerated target detection model to achieve rapid dynamic target recognition. The module supports outputting a 5-centimeter resolution access grid map, with the map update frequency matching the LiDAR output frequency. It can adapt to the complex environment of orchards with tree shade, intertwined branches and leaves, and variable lighting, providing stable and reliable environmental perception results for path planning and avoiding path planning errors caused by perception bias.

[0046] This invention also discloses the specific control logic of the closed-loop tracking control module. The module has a built-in adaptive aiming distance adjustment unit that dynamically adjusts the distance to the path aiming point based on the current travel speed of the agricultural machinery. Higher travel speeds result in a longer aiming distance, and lower travel speeds result in a shorter aiming distance. The aiming distance calculation uses a linear relationship model, based on a fixed low-speed aiming distance, plus the product of travel speed and time gain to obtain the dynamic aiming distance. The module also has a built-in deviation calculation unit that collects the actual position, actual heading, and actual steering angle of the agricultural machinery in real time, calculates the lateral and heading deviations of the agricultural machinery relative to the planned path, and outputs the front wheel angle control quantity using a pure tracking algorithm. The front wheel angle calculation satisfies the kinematic geometry of the agricultural machinery, that is, it calculates the required front wheel angle based on the aiming distance, the angle between the current heading and the aiming point, and uses an incremental PID algorithm to output the travel speed control quantity. The drive motor output power is adjusted according to the deviation between the target speed and the current wheel speed to avoid sudden speed changes. After the path replanning update is completed, the module automatically resets the pre-aiming point position and control parameters to avoid control oscillations and tracking deviations caused by the switching between the old and new paths. This forms a multi-closed-loop control of position, speed, and turning angle, ensuring that the agricultural machinery can track the planned path with high precision and reducing problems such as branch pressing, tree scraping, and missed covering during operation.

[0047] Reference Figure 1This diagram illustrates the overall operational logic of dynamic path planning for precision operations by intelligent agricultural machinery in orchards. The system first acquires and preprocesses orchard data through multi-source sensors, accurately identifying operational boundaries, fixed obstacles, and row parameters of the fruit trees. Then, combining the various physical and operational constraints of the agricultural machinery, the system plans a collision-free initial global operational path covering the entire area. While the machinery operates along this path, the system continuously monitors dynamic obstacles and changes in row width. Once a change in road conditions triggers a safety or passage threshold, the system immediately uses the current position as the starting point and generates a locally replanned path using a multi-generational value fusion algorithm. After confirming that the new path smoothly connects with the remaining global path without collision risk, the system issues the new path, driving the agricultural machinery to safely and efficiently complete subsequent precision operations.

[0048] Reference Figure 2 This diagram details the processing steps from acquiring raw multi-source sensing data to generating a static access grid map. To ensure spatiotemporal consistency of the data, the system performs strict hardware-level time synchronization and unified coordinate transformation on the data from LiDAR, vision, and positioning units. After removing noise and blurred frames, the system constructs an initial environmental perception model. Next, point cloud segmentation technology is used to separate ground and non-ground elements, accurately locating tree trunks and various fixed facilities. By further fitting parallel ridge lines, extracting the centerline of passable areas, and setting safe buffer distances for all obstacles, the system successfully generates a high-precision static access grid map of the orchard, laying a solid foundation for subsequent global path planning.

[0049] Reference Figure 3 This diagram details the steps involved in generating a safe and fully covered initial global work path. The system uses the extracted inter-row passage centerline as the basic framework, ensuring no overlap or omission by setting an overlap threshold for the work width. In turning areas, the system strictly matches the minimum turning radius of the agricultural machinery, using an arc-shaped turning method to generate a smooth path at the edge of the field. To ensure safety, the system actively applies detour offsets on sections near fixed obstacles and performs full-scale collision detection to eliminate all waypoints with potential hazards. Finally, considering the operational efficiency requirements of the agricultural machinery, the system configures a reasonable travel speed for each waypoint along the entire path, thus outputting a complete global work path that can be directly executed.

[0050] Reference Figure 4This figure illustrates the real-time monitoring and replanning triggering mechanism of the system in a dynamic operating environment. During the movement of the agricultural machinery, the sensor module scans in real time for dynamic obstacles in front and to the side, as well as changes in row width caused by the growth of tree canopies. The system calculates the minimum spatial distance from obstacles to the path at high frequency, and the difference between the remaining passage width and the safe width of the agricultural machinery. When a dynamic obstacle is detected to have entered the warning range, or when the remaining passage space is insufficient to ensure safety, the system determines that the path adjustment triggering conditions have been met. At this time, the system immediately truncates the current driving command, locks the real-time pose of the agricultural machinery, and establishes it as the reference starting point for the upcoming local replanning task.

[0051] Reference Figure 5 This figure illustrates the algorithm for generating locally optimal paths and the process for verifying the connection between old and new paths. Based on the newly established replanning starting point, the system explores candidate path points along the passable direction. By independently calculating the operational loss cost due to route deviation, the safety cost of being too close to obstacles, and the path smoothing cost caused by frequent turns, the system assigns weights and derives the total cost. The set of points with the minimum total cost and that meets kinematic constraints is selected to form the replanning path. Subsequently, the system uses cubic B-spline curves to smoothly fit the curvature of the endpoint of this local path to the starting point of the remaining global paths. After rigorous curvature and collision checks, the system outputs continuously updated operational paths.

[0052] The above are merely preferred embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A dynamic planning method for precision operation paths of intelligent agricultural machinery in orchards, characterized in that, Includes the following steps: S1. Acquire multi-source sensing data of the orchard operation area, complete coordinate calibration and data preprocessing using the agricultural machinery operation coordinate system as a unified benchmark, and identify fixed operation boundaries, fixed obstacle locations and fruit tree planting ridge parameters within the operation area; S2. Combining the agricultural machinery's own operating width, minimum turning radius, maximum steering angular velocity, and operating efficiency constraint parameters, generate a collision-free initial global operating path covering the entire operating area, and send the initial path to the agricultural machinery operating control terminal. S3. During the operation of agricultural machinery along the initial path, real-time information on dynamic obstacles in front of and to the side of the operation path, real-time information on changes in the crown width of fruit trees and changes in the width of the operation ridges is collected. The dynamic obstacle information includes the location of the operators, the location of temporary stacked agricultural materials and the location of moving targets that have entered the operation area. Based on preset judgment rules, it is identified whether the path dynamic adjustment threshold is triggered. S4. When the path adjustment triggering condition is met, the current real-time position of the agricultural machinery is used as the starting point for replanning. A dynamic planning cost function is constructed that integrates the cost of operation loss, the cost of passage safety, and the cost of path smoothing. The local optimal replanning path is selected by combining the kinematic constraints of the agricultural machinery. S5. Complete the smooth connection verification between the replanned path and the remaining global path, check for collision risks and curvature change problems in the connecting path, and send the verified connecting path to the operation control terminal to drive the agricultural machinery to complete the remaining area operation along the updated path until the entire area operation is covered.

2. The method for dynamic planning of precision operation paths for intelligent agricultural machinery in orchards according to claim 1, characterized in that, The agricultural machinery operation coordinate system is a unified reference for coordinate calibration and data preprocessing, specifically including: hardware-level time synchronization of sensing data from different sources; conversion of raw data collected by each sensor to a unified operation coordinate system based on pre-calibrated sensor extrinsic parameters; removal of outlier noise points, airborne object interference points, and motion-blurred frames from point cloud data and image data; completion of feature matching and region stitching of adjacent frame sensing data; and construction of an initial environmental perception model of the operation area.

3. The method for dynamic planning of precision operation paths for intelligent agricultural machinery in orchards according to claim 2, characterized in that, The identification of fixed work boundaries, fixed obstacle locations, and fruit tree planting ridge parameters within the work area specifically includes: segmenting the preprocessed point cloud data into ground points and non-ground points; identifying the locations of fruit tree trunks, fixed irrigation facilities, guardrail boundaries, and field power poles based on the height clustering results of non-ground points; fitting parallel ridge lines of adjacent fruit tree rows; extracting the centerline of the passable area between ridges; measuring the geometric dimensions of each fixed obstacle and setting corresponding safety buffer distances; and generating a static passable grid map of the work area.

4. The method for dynamic planning of precision operation paths for intelligent agricultural machinery in orchards according to claim 1, characterized in that, The specific steps for generating a collision-free initial global operation path covering the entire operation area include: based on the requirement of full coverage operation without duplication or omission, using the center line of the inter-row passage area as the basic path, using an arc-shaped turning connection method to match the minimum turning radius of the agricultural machinery to generate the field turning path, setting the width overlap threshold of adjacent operation zones based on the operation width, setting a safe detour offset for path segments close to fixed obstacles, eliminating waypoints with collision risks in the path, and setting the preset driving speed for each waypoint to form a complete initial global operation path.

5. The method for dynamic planning of precision operation paths for intelligent agricultural machinery in orchards according to claim 1, characterized in that, The specific steps for identifying whether to trigger the path dynamic adjustment threshold based on preset judgment rules include: real-time calculation of the minimum spatial distance between the perceived dynamic obstacle and the current driving path, and the difference between the remaining passage width of the row caused by the outward shading of the fruit tree canopy and the safe passage width of the agricultural machinery. When the dynamic obstacle enters the preset safety warning range, or the remaining passage width of the row is less than the safe passage width threshold of the agricultural machinery, it is determined that the path adjustment trigger condition is met, the current driving command is paused simultaneously, and the current position and posture parameters of the agricultural machinery are locked as the replanning reference.

6. The method for dynamic planning of precision operation paths for intelligent agricultural machinery in orchards according to claim 1, characterized in that, The dynamic programming cost function for constructing the fusion operation loss cost, traffic safety cost, and path smoothing cost adopts the following expression: ;in, The total value of candidate path points. , , These are the weighting coefficients for the operation loss cost, traffic safety cost, and path smoothing cost, respectively, and the sum of the three weighting coefficients is 1; The cost of lost work is the cost of subsequent rerouting work calculated based on the area of ​​unworked areas caused by the deviation of the original work zone from the work route. The collision risk cost is calculated based on the minimum distance between the candidate path point and surrounding obstacles and fruit tree trunks, in order to ensure safe passage. The cost of path smoothing is the agricultural machinery steering control cost calculated based on the change in heading angle between adjacent candidate path points.

7. The method for dynamic planning of precision operation paths for intelligent agricultural machinery in orchards according to claim 6, characterized in that, The verification of the smooth connection between the completed replanned path and the remaining global path specifically includes: using a cubic B-spline curve to continuously fit the curvature of the end point of the replanned path and the starting point of the remaining global path; verifying point by point whether the curvature value of the fitted path segment is within the curvature threshold range corresponding to the minimum turning radius of the agricultural machinery; checking whether the fitted path segment has a collision risk with surrounding obstacles; generating a continuous updated operation path after the verification is passed; recording the operation coverage of the replanned location; and generating supplementary operation paths for the missed operation areas caused by replanning during the final stage of the full-area operation.

8. A dynamic planning system for precision operation paths of intelligent agricultural machinery in orchards, used to implement the dynamic planning method for precision operation paths of intelligent agricultural machinery in orchards as described in any one of claims 1-7, characterized in that, It includes a multi-source data perception module, a global path planning module, a dynamic trigger judgment module, a local path replanning module, a path connection verification module, and a job execution control module; The multi-source data sensing module is used to acquire multi-source sensing data of the orchard operation area, and complete coordinate calibration and data preprocessing with the agricultural machinery operation coordinate system as a unified reference, and identify the fixed operation boundary, fixed obstacle position and fruit tree planting ridge parameters within the operation area. The global path planning module is used to combine the agricultural machinery's own working width, minimum turning radius, maximum steering angular velocity, and working efficiency constraint parameters to generate a collision-free initial global working path covering the entire working area and send it to the working control terminal. The dynamic trigger judgment module is used to collect information on dynamic obstacles in front of and to the side of the operation path, real-time changes in the crown width of fruit trees and changes in the width of the operation ridges in real time during the operation of agricultural machinery along the initial path, and to identify whether to trigger the dynamic adjustment threshold of the path based on preset judgment rules. The local path replanning module is used to construct a dynamic planning cost function that integrates operation loss cost, traffic safety cost, and path smoothing cost when the path adjustment triggering condition is met, taking the current real-time position of the agricultural machinery as the replanning starting point, and combining the kinematic constraints of the agricultural machinery to select the local optimal replanning path. The path connection verification module is used to complete the smooth connection verification between the replanned path and the remaining global path, check the collision risk and curvature change problem of the connecting path, and send the verified connecting path to the operation execution control module. The operation execution control module is used to drive the agricultural machinery to complete the remaining area operation along the updated path until the entire area operation is covered.

9. The orchard intelligent agricultural machinery precision operation path dynamic planning system according to claim 8, characterized in that, The multi-source data sensing module includes an airborne differential positioning unit, a three-dimensional lidar sensing unit, a panoramic vision acquisition unit, and an agricultural machinery posture detection unit. Each unit achieves microsecond-level alignment of data acquisition timestamps through a hardware time synchronization trigger line. It achieves spatial coordinate transformation of data from different sensors through a pre-calibrated sensor extrinsic parameter matrix. The built-in point cloud statistical filtering algorithm and image motion blur removal algorithm complete the quality optimization of the original sensing data.

10. The orchard intelligent agricultural machinery precision operation path dynamic planning system according to claim 8, characterized in that, The operation execution control module has a built-in path tracking deviation calibration unit, which is used to collect the lateral and heading deviations between the actual driving trajectory of the agricultural machinery and the planned path in real time. The driving speed and steering angle of the agricultural machinery are adjusted through the pre-aiming tracking control algorithm. After each path replanning is completed, the pre-aiming point position and control parameters are updated synchronously to achieve high-precision tracking of the planned path.

Images