An autonomous navigation system and method for a tracked orchard plant protection robot

Abstract

The application discloses a kind of orchard plant protection robot autonomous navigation system and method, belong to agricultural robot navigation technical field, this method includes obtaining and analyzing the field information containing fruit tree row distance, plant distance, electronic fence coordinates and operation speed;According to the row-by-row scanning planning of electronic fence and row distance, the head-turning path is generated by using double-circle incircle, and is spliced into global path;When operating, translation deviation is carried out according to the fruit tree row position identified by the perception module, and when encountering obstacles, the obstacle avoidance path or stop obstacle is generated based on A search algorithm and B spline optimization;After deciding the path to be tracked, transverse tracking and longitudinal speed control are performed, longitudinal control adds pitch angle slope compensation to realize climbing, speed reduction and torque increase, and deceleration at the turning point of head.

Description

Technical Field

[0001] This invention relates to the field of agricultural robot navigation technology, specifically to an autonomous navigation system and method for a tracked orchard plant protection robot. Background Technology

[0002] Orchard plant protection is a crucial aspect of orchard production management. Considering that most orchards are located on undulating terrain, tracked chassis are widely used in orchard environments such as hilly areas, gentle slopes, and soft soils due to their good passability and strong terrain adaptability.

[0003] Taking the patent document with publication number CN119586594A as an example, it discloses a fully hydraulic tracked orchard plant protection robot equipped with GPS-RTK, lidar and vision sensors for navigation, but this prior art solution still has the following shortcomings: First, navigation path generation relies on line-by-line calibration. The existing method requires technicians to mark or record the trajectory of each row of fruit trees before the operation. The more rows there are, the greater the calibration workload. In addition, the fixed path cannot adapt to the changes in row spacing caused by the growth of fruit trees and lacks long-term applicability.

[0004] Secondly, it lacks complete decision-making capabilities for turning around and avoiding obstacles at the edge of the field; the existing system only focuses on inter-row following operations, and after reaching the edge of the field, the causal tree features are missing and navigation cannot continue, and there is a lack of specific and feasible turning planning schemes; the curvature of the path generated for obstacle avoidance is discontinuous, making it difficult for the tracking and control module to execute effectively.

[0005] Third, the control dimension is singular and the terrain adaptability is poor; the existing control strategy is mainly based on lateral control and lacks longitudinal speed coordination adjustment. The tracked chassis relies on differential steering, which has a high slip coefficient. At the turning point, deceleration logic needs to be added to prevent tracking deviation; at the same time, the orchard has cross slopes, longitudinal slopes or mixed slopes, so longitudinal adaptive control strategies need to be added to enhance climbing ability, and composite control strategies such as on-the-spot turning need to be added to reduce the risk of rollover. Summary of the Invention

[0006] The purpose of this invention is to provide an autonomous navigation system and method for tracked orchard plant protection robots, which solves the problems of existing orchard plant protection robot navigation systems, such as path generation relying on line-by-line calibration, lack of complete decision-making capabilities for turning around and obstacle avoidance, and poor terrain adaptability due to a single control dimension.

[0007] To achieve the above objectives, the present invention provides the following technical solution: an autonomous navigation method for a tracked orchard plant protection robot, comprising: Acquire and parse plot information, which includes row spacing and plant spacing of fruit tree rows, coordinate point information of electronic fence, expected operation speed and turning speed at the edge of the field; Based on the electronic fence and the row spacing of the fruit tree rows, inter-row path planning is performed within the electronic fence, and a double-circle in-circle method is used to generate a field-end turning path. The inter-row path and the field-end turning path are then spliced ​​together to form a global path covering all fruit tree rows. During the robot's journey along the global path, the position information of the fruit tree rows identified by the perception module is acquired in real time, the lateral deviation and heading deviation are calculated, and the global path is corrected based on the lateral deviation and heading deviation; when an obstacle is detected, the drivable area information is acquired, and an obstacle avoidance path is generated or a stop command is issued based on the drivable area information. The robot's real-time location points determine the path to be followed from the current safe driving path; Lateral tracking control and longitudinal speed control are performed on the path to be tracked, and control commands are output to the left and right track actuators. The lateral tracking control is based on the geometric relationship of the pre-aiming point and calculates the desired angular velocity according to the pre-aiming distance and lateral error. The longitudinal speed control adopts a closed-loop control strategy, superimposing a slope compensation amount based on the pitch angle on the control quantity to achieve climbing speed reduction and torque increase, and decelerating to the turning speed when the turning point is detected.

[0008] A further technical solution, specifically includes the step of concatenating the inter-row path and the field-end turning path into a global path covering all fruit tree rows, which includes: Based on the electronic fence, fence points perpendicular to both sides of the fruit tree row are extracted, and polynomial curve fitting is performed on the fence points to obtain fence boundary information; Based on the row spacing of the fruit tree rows, the plots within the electronic fence are scanned row by row to obtain the coordinate point information of each fruit tree row. The midpoint of the coordinate points of adjacent fruit tree rows is taken to obtain the intermediate work point information of adjacent fruit tree rows. Based on the start and end point pair information of each work row, the turning-around path between adjacent work rows is generated using the double-circle in-circle method; The global path is generated by concatenating the inter-row paths of each work row with the turning-off path at the edge of the field after interpolation and encryption.

[0009] A further technical solution includes the step of correcting the global path based on the lateral deviation and heading deviation, comprising: After the robot enters the course, it travels along the global path and obtains the lateral deviation and heading deviation output by the perception module in real time, and removes noise and outliers in the lateral deviation and heading deviation. Based on the removed lateral and heading deviations, the global path is translated to move the robot from its current position toward the translated path. Continuously monitor lateral and heading deviations, and repeat the translation operation when the deviation exceeds a preset threshold. When the robot reaches the starting point marker, it turns off the correction function and generates a turning path according to the double-circle in-circle method to complete the turning.

[0010] A further technical solution includes the step of generating an obstacle avoidance path based on the drivable area information, which includes: From A Multiple points are sampled on the path generated by the search algorithm and used as the initial control points of the cubic uniform B-spline curve. A cost function is constructed, which includes a smoothness cost and a collision cost. The optimized control points are obtained by optimizing the cost function, and the obstacle avoidance path is generated from the optimized control points. The smoothness cost is characterized by elastic band energy: ; Among them, Q i Let n be the i-th control point, and n be the total number of control points. The collision cost is represented by a potential field, and the single-point collision cost is: ; Where d(p) is the distance from point p to the nearest obstacle, d safe For the safety boundary distance, d max This represents the distance to the inner boundary of the danger zone.

[0011] A further technical solution is that the cost function is: ; ω smooth For smoothness weights, ω collision For collision weights, f collision (Q) is the sum of the single-point collision costs of the four boundary points after modeling the robot as a rectangle.

[0012] In a further technical solution, the desired angular velocity in the lateral tracking control is calculated according to the following formula: ; Where, ω des For the desired angular velocity, v des Here, is the desired linear velocity, err is the lateral error, and dist is the aiming distance.

[0013] In a further technical solution, the slope compensation amount in the longitudinal speed control is: g·sin(pitch); Where g is the acceleration due to gravity, pitch is the pitch angle, and when the robot is climbing, the slope compensation is subtracted from the control quantity to achieve speed reduction and torque increase.

[0014] A further technical solution includes, in addition to, a stationary steering control mode for performing lateral tracking control and longitudinal speed control on the path to be tracked. When a ground marker is detected, the robot decelerates to zero and then uses a closed-loop control algorithm to control the robot's heading angle to achieve a turn in place.

[0015] In a further technical solution, in the step of acquiring and parsing the land parcel information, the coordinate point information of the electronic fence is converted from the 84 coordinate system to the Northeast-Sky local coordinate system, and the global path is generated under the Northeast-Sky local coordinate system.

[0016] An autonomous navigation system for a tracked orchard plant protection robot includes: The information configuration and parsing module is used to acquire and parse plot information, which includes the row spacing and plant spacing of fruit tree rows, the coordinate point information of the electronic fence, the expected operation speed, and the turning speed at the edge of the field. The global path planning module is used to plan the inter-row path within the electronic fence based on the row spacing of the electronic fence and the fruit tree rows, and to generate the field-end turning path using a double-circle in-circle method. The inter-row path and the field-end turning path are then spliced ​​together to form a global path that covers all fruit tree rows. The perception module is used to identify the position of the fruit tree rows and obstacle information in real time, and output the lateral deviation and heading deviation. The local path correction and obstacle avoidance planning module is used to translate and correct the global path based on the lateral deviation and heading deviation, and to obtain drivable area information when an obstacle is detected, and to generate an obstacle avoidance path or issue a stop command based on the drivable area information. The decision-making module is used to determine the path to be tracked from the current safe driving path based on the robot's real-time positioning points; The tracking control module is used to perform lateral tracking control and longitudinal speed control on the path to be tracked, and output control commands for the left and right track actuators. The lateral tracking control is based on the geometric relationship of the pre-aiming point and calculates the desired angular velocity according to the pre-aiming distance and lateral error. The longitudinal speed control adopts a closed-loop control strategy, superimposing a slope compensation amount based on the pitch angle on the control quantity to achieve speed reduction and torque increase when climbing, and decelerating to the turning speed when the turning point is detected.

[0017] In summary, the present invention has the following beneficial effects: 1. Reduces pre-operation calibration costs and adapts to changes in fruit tree growth. This invention eliminates the need for technicians to mark points or record trajectories for each fruit tree row before operation. Users only need to use a remote control to define an electronic fence and input the row spacing to automatically generate a global path covering all fruit tree rows. During operation, the sensing module identifies the position of the fruit tree rows in real time and corrects the global path based on lateral and directional deviations. It can adapt to actual deviations in row spacing and changes in row spacing caused by fruit tree growth, thus having long-term applicability.

[0018] 2. Possesses full-process autonomous navigation and intelligent decision-making capabilities. This invention employs a double-circle inscribed method to achieve automatic turning-around planning at the edge of the terrain, solving the problem of navigation failure when edge features are missing; it also uses A... The method combining the search algorithm with cubic uniform B-spline curve optimization enables automatic obstacle avoidance or stopping decisions, and has a complete autonomous navigation capability for the entire process of "driving between rows - turning around at the end of the road - obstacle avoidance / stopping".

[0019] 3. Integrating lateral and longitudinal control, this invention offers strong terrain adaptability. Building upon lateral tracking control based on the geometric relationship of the pre-aiming point, it adds longitudinal control based on pitch angle-based slope compensation. During uphill climbing, slope compensation reduces speed and increases torque, enhancing climbing performance; during downhill climbing, it prevents speeding. Simultaneously, it executes deceleration logic upon identifying turning points at the edge of the terrain, reducing the risk of rollover; and a stationary turning control mode is designed for special terrains with high elevation edges, further enhancing safety in all operating scenarios.

[0020] 4. The obstacle avoidance path is smooth and trackable. This invention will... The search algorithm is combined with cubic uniform B-spline curve optimization, constructing a cost function with smoothness and collision costs. Through optimization, a smooth, safe, and curvature-continuous obstacle avoidance path is generated, overcoming the limitations of traditional A / B optimization methods. The search algorithm suffers from the drawback of discontinuous path curvature, which hinders subsequent path tracing. Attached Figure Description

[0021] The accompanying drawings, which are included to provide a further understanding of this application and form part of this application, illustrate exemplary embodiments of this application and are used to explain this application, but do not constitute an undue limitation of this application. In the drawings: Figure 1 This is a schematic diagram illustrating the working principle of this application; Figure 2 This is a schematic diagram of the turning path generation in this application; Figure 3 This is a global path diagram of this application; Figure 4 This is a schematic diagram of partial path correction in this application; Figure 5This is a local obstacle avoidance effect diagram based on a grid map in this application; Figure 6 This is a schematic diagram of the pure tracking algorithm in this application. Detailed Implementation

[0022] To more clearly illustrate the overall concept of this application, a detailed explanation is provided below with reference to the accompanying drawings.

[0023] Many specific details are set forth in the following description in order to provide a full understanding of this application. However, this application may also be implemented in other ways different from those described herein. Therefore, the scope of protection of this application is not limited to the specific embodiments disclosed below.

[0024] Furthermore, it should be understood in the description of this application that the terms "top", "bottom", "inner", "outer", "axial", "radial", "circumferential", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, and are only for the convenience of describing this application and simplifying the description, and are not intended to indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the present invention.

[0025] In this application, unless otherwise expressly specified and limited, the terms "installation," "connection," "linking," and "fixing," etc., should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral part; they can refer to a mechanical connection, an electrical connection, or a communication connection; they can refer to a direct connection or an indirect connection through an intermediate medium; they can refer to the internal communication of two components or the interaction between two components. Those skilled in the art can understand the specific meaning of the above terms in this invention according to the specific circumstances.

[0026] In this application, unless otherwise expressly specified and limited, the "above" or "below" of the second feature can mean that the first and second features are in direct contact, or that the first and second features are in indirect contact through an intermediate medium. In the description of this specification, references to terms such as "an embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of this application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described can be combined in any suitable manner in one or more embodiments or examples.

[0027] I. General Description This invention provides an autonomous navigation system and method for a tracked orchard plant protection robot, which is integrated into the onboard controller of the tracked orchard plant protection robot. Figure 1 As shown, the system mainly includes an information configuration and parsing module, a global path planning module, a perception module, a local path correction and obstacle avoidance planning module, an intelligent decision-making module, and a tracking and control module.

[0028] In one embodiment, the robot employs a tracked chassis mechanism, with each of the left and right tracks driven by independent drive motors, achieving steering through the differential speed of the two tracks. The robot is equipped with a lidar as its primary sensing sensor, used to collect real-time information on tree rows and obstacles; a Global Navigation Satellite System (GNSS) receiver (such as GPS-RTK) and an inertial measurement unit (IMU) to acquire the robot's real-time positioning and attitude information (including pitch, roll, and yaw angles); an industrial control computer as the upper-level controller, responsible for path planning, sensing data processing, decision-making, and the execution of tracking control algorithms; and a lower-level VCU (vehicle control unit) responsible for receiving control commands from the upper level and driving the left and right track motors. The robot is also equipped with a remote control for users to configure and input site information before operation.

[0029] II. Information Acquisition and Analysis Before the Task: 2.1 Land Parcel Information Input Before the plant protection operation begins, the user selects and enters the corresponding plot information through the robot's remote control. Specifically, the user enters the following information in sequence through the human-machine interface of the remote control, following the prompts: row spacing of fruit trees (unit: meters), plant spacing of fruit trees (unit: meters), coordinates of the plot's electronic fence (using the WGS-84 coordinate system, i.e., latitude and longitude coordinates under the 84 coordinate system), the user's desired operating speed (unit: meters / second), turning speed at the edge of the field (unit: meters / second), and spray pressure (unit: megapascals), etc.

[0030] In one embodiment, the coordinates of the electronic fence are recorded by a user walking around the boundary of the plot using a handheld remote control. During recording, the user is required to start from the side perpendicular to the rows of fruit trees and sequentially collect the latitude and longitude coordinates of several boundary points along the plot boundary. The electronic fence is a free-form curve, and the starting point for recording each point must be along the direction of the rows of fruit trees.

[0031] 2.2 Configuration File Generation and Parsing After the user completes the above information entry, the remote control packages all the land parcel information into a JSON-formatted configuration file. This file is then transmitted to the robot's industrial control computer. Before starting the operation, the industrial control computer first parses the configuration file and extracts the various parameters for use by the subsequent path planning and decision-making modules.

[0032] Since subsequent global path planning is performed in the East-North-Up (ENU) local coordinate system, while the original coordinates of the electronic fence are in the latitude and longitude coordinates of the 84 coordinate system, coordinate transformation is required during the analysis process. Specifically, the coordinates of each point of the electronic fence are transformed from the 84 coordinate system to the East-North-Up local coordinate system. The transformation method is as follows: select any point within the electronic fence area (such as the first fence point or the center point of the plot) as the origin of the East-North-Up coordinate system, calculate the eastward and northward offsets of each fence point relative to this origin, and obtain the two-dimensional plane coordinates in the East-North-Up coordinate system. After the coordinate transformation is completed, all subsequent path planning, positioning, and tracking control are performed in the East-North-Up local coordinate system.

[0033] In one embodiment, if GPS signal obstruction leads to a decrease in positioning accuracy, IMU data can be fused for dead reckoning to maintain continuous positioning of the robot in the northeast-northeast coordinate system.

[0034] III. Global Path Planning 3.1 Fence Boundary Fitting After parsing the job information, the global path planning module plans the inter-row paths and field-end turning paths based on the electronic fence information and the row spacing of the fruit tree rows, so as to ensure high coverage of all fruit tree rows within the electronic fence and ensure that no job is missed.

[0035] First, based on the input coordinates of the electronic fence, points perpendicular to both sides of the fruit tree rows are extracted. Specifically, since the recording starting point of the electronic fence is along the direction of the fruit tree rows, the boundary points perpendicular to the direction of the fruit tree rows in the fence point sequence can be identified. Polynomial curve fitting is then performed on these fence points to obtain the fence boundary information. In one embodiment, a cubic polynomial curve is used for fitting to smooth the direction of the fence boundaries.

[0036] 3.2 Line-by-line scanning and job row generation Then, based on the user-input row spacing *d*, the area within the electronic fence is scanned row by row. The scanning method is as follows: starting from one boundary of the electronic fence, the robot advances gradually along a direction perpendicular to the row spacing *d*, advancing one row spacing at a time to determine the position of one row of trees. After this step is completed, the coordinate point information of each row of trees can be obtained—that is, the sequence of position points of each row of trees in the northeast-northeast coordinate system. Taking the midpoint of the coordinate point sequence of two adjacent rows of trees yields the intermediate work point information of the adjacent rows—that is, the actual work path point of the robot's movement.

[0037] 3.3 Generation of U-turn Path at the End of the Field (using the double-circle inscribed method) Next, based on the start and end point pair information of each work row (i.e., the coordinates of the start and end points of each work row), the turning path between adjacent work rows is determined. This invention uses a double-circle in-circle method to generate the turning path between adjacent fruit tree rows.

[0038] like Figure 2 As shown, suppose the previous job line is located at Figure 2 Below the middle, the next job line is located Figure 2 Above the rows, the vertical distance between the two rows is d (i.e., the row spacing of the fruit trees, input by the user). Point A is the end point (end of the field) of the previous row, and point B is the starting point (end of the field) of the next row. In the diagram, orA and orB are the centers of the two inscribed circles, and rA and rB are the radii of the two circles, respectively. The center of the larger circle is orA and its radius is rA, while the center of the smaller circle is orB and its radius is rB, i.e., rA > rB. The smaller circle is located inside the larger circle, and the two circles are internally tangent in the inter-row area. θ is an angle parameter that the user can set to control the curvature characteristics of the turning path. The tangent point is the point of tangency between the two circles. The curve from point A through the arc to the tangent point and then through the arc to point B is the generated turning path. In one embodiment, the value of θ ranges from 30° to 60°.

[0039] The two centers, orA and orB, are uniquely determined by the following geometric method: (1) Determine orB Starting from point A, draw a ray in a direction perpendicular to the previous row and pointing to the next row (i.e., perpendicular to the fruit tree row and pointing to the inter-row area, upward in the figure). Rotate this ray clockwise by an angle θ with point A as the axis to obtain ray l_c.

[0040] Starting from point B, draw a ray (the perpendicular line from point B) in a direction that is perpendicular to the next row and points to the previous row (i.e., perpendicular to the fruit tree row and pointing to the inter-row area, which is downward in the figure).

[0041] The intersection of ray l_c and the perpendicular line emanating from point B is orB. Therefore, rB = |orB|. B| represents the distance from point B to point B.

[0042] (2) Determine orA orA lies on a ray originating from point A, running perpendicular to the previous work row and pointing towards the next work row (upwards in the diagram). Let the distance from orA to point A be rA.

[0043] Since the smaller circle (center or B, radius rB) lies inside the larger circle (center or A, radius rA), the two circles are internally tangent in the inter-circle region. Therefore, the distance between the two centers is equal to the difference between the two radii, i.e.: |orA orB| = rA rB Since orA = A + rA·u (where u is the unit direction vector pointing vertically upwards from point A to the inter-row region, and since orA is perpendicular to A, u is upwards and has a value of 1), orB and rB have been determined in step (1). Therefore, substituting orA = A + rA·u into |orA orB| = rA In rB, we obtain |(A + rA·u) orB| = rA Since rB is the variable, only rA is unknown in the above equation. Solving this single-variable equation will yield the value of rA, thus uniquely determining the position of orA.

[0044] (3) Determine the tangent point tangent Since the two circles are internally tangent, the point of tangency lies on the extension of the line connecting orA and orB. Specifically: tangent = orA + rA·(orB orA) / |orB orA| That is, the point reached by moving a distance rA from orA along the direction orA→orB is the point of intimacy between the two circles. This point is also the point reached by moving a distance rB from orB along the direction orB→orA.

[0045] (4) Generate a double circular arc path Starting from point A, travel along the corresponding arc segment of the large circle (center or A, radius rA) to point Tangent, then travel along the corresponding arc segment of the small circle (center or B, radius rB) to point B, thus forming a complete and smooth turning path from the end of the previous work row to the starting point of the next work row.

[0046] In the geometric configuration of this invention, the relationship between rA and rB depends on the specific input parameters (d, θ), and there is no need to pre-assume which is larger or smaller. The above derivation uses absolute value form and automatically applies to two cases. If rA > rB, then orA is the center of the larger circle and orB is the center of the smaller circle, with the smaller circle inside the larger circle; if rB > rA, then orB is the center of the larger circle and orA is the center of the smaller circle, with the smaller circle inside the larger circle. In both cases, the inscribed equation |orA - orB| = |rA - rB| holds.

[0047] 3.4 Path Concatenation and Encryption Finally, the straight paths between rows and the U-turn paths of each work row are interpolated and then concatenated to form a complete global path. The purpose of interpolation and encryption is to ensure that the spacing between path points is small enough to guarantee the smoothness of subsequent path tracking control. Specifically, a series of path points are generated by interpolating the straight segments between rows at equal intervals, and a series of path points are generated by sampling the U-turn arc segments at equal angular intervals. Then, all path points are connected sequentially according to the driving order to obtain the following... Figure 3 The global path shown includes a complete road network of straight segments between rows and circular segments at the edge of the field. P1 to P5 in the figure are the endpoints of the electronic fence, which are connected in sequence to form the work plots. The internal parallel line segments are the inter-row paths of each work row generated by scanning line by line, and the curves are the turning paths at the edge of the field generated by using the double circle in-circle method.

[0048] It is important to note that the global path generated at this stage is based solely on geometric planning using the electronic fence and the row spacing input by the user, without considering the deviation between the actual tree row positions and the ideal row spacing. This global path will be corrected subsequently using real-time information from the sensing module.

[0049] IV. Local Path Correction and Obstacle Avoidance Planning 4.1 Path correction based on perception information Considering that fruit trees in actual orchards are planted manually, the actual row spacing will inevitably deviate from the theoretical row spacing input by the user. If the operation is performed strictly according to the aforementioned global path, the robot may collide with the fruit trees as the deviation accumulates. Therefore, this invention designs a local path correction mechanism based on real-time information from the perception module.

[0050] After the robot initiates autonomous operation and enters the row, the perception module uses LiDAR to collect point cloud information of the fruit trees on both sides in real time. Specifically, the LiDAR scans and acquires two-dimensional or three-dimensional point cloud data of the surrounding environment. The point cloud data is then processed using clustering algorithms (such as density-based DBSCAN clustering or Euclidean clustering), grouping point clouds belonging to the same row of fruit trees into one category, thereby identifying the positions of the fruit tree rows on both sides. By taking the midline between the positions of the fruit tree rows on both sides, the centerline position of the current actual working row can be obtained.

[0051] The robot's current position represents its corresponding position on the original global path. Therefore, based on the robot's real-time positioning point (obtained through GPS-RTK and IMU fusion positioning) and the actual work row centerline fitted by the perception module, the lateral deviation (the vertical distance from the robot's current position to the work row centerline) and the heading deviation (the angle between the robot's current orientation and the direction of the work row centerline) can be calculated in real time. By translating and correcting the aforementioned global path based on these lateral and heading deviations, the deviation in global path generation caused by non-standard planting spacing of fruit trees can be compensated.

[0052] 4.2 Correction Process like Figure 4 As shown, the specific process for local path correction is as follows: After the robot enters the system, it first undergoes a brief online process following the original global path (i.e., the process of moving from the current position towards the original path). Simultaneously, it continuously monitors the lateral deviation and heading deviation data output by the perception module, removing noise and outliers from the data. Specifically, median filtering or Kalman filtering can be used to smooth the deviation data and remove abnormal jumps caused by perception noise.

[0053] Then, the original global path is translated based on the filtered lateral and heading deviations. The direction and magnitude of the translation are determined by the current deviation—if the robot is deviated to the left by Δx meters relative to the work row centerline, the global path is translated to the right by Δx meters; if there is a heading deviation, a rotation correction is performed simultaneously. After the translation is completed, the robot will generate an alignment process from its current position to the translated path, that is, the robot automatically adjusts its travel direction to align with the corrected path.

[0054] After going live, the robot continues to monitor lateral and heading deviations. If the deviation exceeds a preset threshold (e.g., a lateral deviation threshold of 0.1 meters and a heading deviation threshold of 5 degrees), a new round of translation operations is triggered to correct the path again. This process continues during row-to-row movement to ensure that the robot always travels along the centerline of the actual fruit tree row.

[0055] When the robot reaches the end-of-field marker (determined by the starting point of the turn in the global path), since the robot has already left the fruit tree row area, the perception module has acquired less feature information about the fruit tree row and cannot accurately output lateral and heading deviations. Therefore, the correction function is disabled. Then, the robot generates a turning path based on the coordinates of the turning points of adjacent work rows using the double-circle in-circle method described above, completing the turning plan. After turning around, the robot enters the next work row, continues to monitor the lateral and heading deviation data, and repeats the above correction process.

[0056] 4.3 Obstacle Detection and Drivable Area Analysis In actual orchard operations, there may be static obstacles such as utility poles and irrigation canals between rows and at the edges of the field, as well as dynamic obstacles such as workers and other vehicles. Therefore, based on the aforementioned correction path, the perception module needs to detect obstacles on the robot's working path in real time.

[0057] Specifically, the LiDAR scanner scans the environmental point cloud in front of the robot in real time, and identifies the location and contour information of obstacles using obstacle detection algorithms (such as grid-based obstacle detection or point cloud clustering methods). After detecting an obstacle, a local grid map is invoked to detect drivable areas. The grid map divides the robot's surrounding environment into several fixed-size grid units (e.g., each grid is 0.05m × 0.05m), and each grid is marked as "occupied" (obstacles exist) or "free" (passable) based on the LiDAR point cloud data.

[0058] If the drivable area can meet the robot's safe passage (i.e., there is a drivable passage with sufficient width for the robot to pass through), then the obstacle avoidance algorithm is invoked to generate a local obstacle avoidance path; if the drivable area does not meet the robot's safe passage (e.g., the passage width is less than the robot's width plus safety redundancy), then an emergency stop command is issued to the robot, waiting for manual intervention or the removal of the obstacle.

[0059] 4.4 Obstacle Avoidance Path Generation This invention uses A A method combining a search algorithm with cubic uniform B-spline curve optimization generates a robot-trackable obstacle avoidance path. The search algorithm is a common grid map search algorithm used in engineering to find the shortest path from a starting point to an end point on a grid map. A The specific implementation of the search algorithm is a well-known technology in this field and will not be elaborated here.

[0060] From A The sampled points along the path generated by the search algorithm are used as the initial control points for the cubic uniform B-spline curve. Let the control point sequence be Q0, Q1, …, Q {n-1} Where n is the total number of control points. Next, optimization rules are designed to optimize these initial control points to meet the application scenario requirements of this invention.

[0061] (1) Endpoint constraints To ensure that the robot's actual path smoothly matches the global path at the start and end points of local obstacle avoidance, the tangent direction at the start point should be parallel to vector Q1. Q0, the direction of the tangent at the endpoint should be parallel to vector Q. {n-1} Q {n-2} Furthermore, to ensure that the local obstacle avoidance path can successfully revert to the global path, a point Q on the global path is selected as the local path recovery point. {n-1} So, Q {n-2} =Q {n-1} L·d des Where L is the step size (i.e., the distance between adjacent control points), and d desThis is the direction vector of the recovery point, which is obtained directly from the global path.

[0062] (2) Construction of cost function The local obstacle avoidance path required for the application scenario of this invention does not include time, velocity, and acceleration terms; therefore, only smoothness cost and collision cost are considered in the optimization terms. The cost function is expressed as follows: ; Where, ω smooth and ω collision These are the smoothness weight and the collision weight. In actual path generation, these two weights can be adjusted autonomously according to the specific operating scenario. If the rows in the orchard are narrow and obstacles are dense, the collision weight can be appropriately increased to ensure safety; if the rows are wide and there are fewer obstacles, the smoothness weight can be appropriately increased to improve driving comfort. By adjusting the ratio of the two weights, a balance can be achieved between obstacle avoidance performance and path tracking performance.

[0063] (3) Smoothing cost The cost of smoothness is characterized by elastic band energy: ; The above formula represents the relationship between three adjacent control points (Q) {i-1} Q i and Q {i+1} The degree of curvature of the three control points. When the three control points are collinear, this value is 0, and the path is smoothest; the greater the curvature of the three control points, the larger this value is, and the less smooth the path is.

[0064] (4) Collision cost Collision cost is characterized by an artificial potential field. The single-point collision cost is expressed as: ; Where d(p) represents the distance from a point to the nearest obstacle; d safe This represents the safety boundary distance, which in this invention is the sum of half the vehicle width, the safety redundancy, and the positioning error. For example, in one embodiment, if the vehicle width is 1.2 meters, the safety redundancy is set to 0.3 meters, and the positioning error is set to 0.1 meters, then d... safe =0.6 + 0.3 + 0.1 = 1.0 meter; d max This represents the maximum impact distance, which is the inner boundary of the danger zone, and its value is d. safe δ, where δ is a small bandwidth parameter that can be determined based on the actual debugging effect. In one embodiment, δ is taken as 0.2 meters.

[0065] Based on the above definition, the analysis results of single-point collision cost are as follows: If d(p) ≥ dsafe The cost is 0, and point p is completely in the safe zone at this time, requiring no penalty. If d(p) ≤ d max , representing that point p has approached or entered the safe boundary, the penalty value is set to 1 (maximum penalty); If d max <d(p)<d safe The penalty value changes continuously between 0 and 1, and the closer to the obstacle, the greater the penalty.

[0066] The robot is modeled as a rectangle (its length and width are consistent with the actual size of the robot). Single-point collision cost calculations are performed at the four boundary points of the rectangle (i.e., the four corner points of the robot). The costs at the four points are then summed to obtain the total collision cost f. collision (Q).

[0067] Substituting both smoothness and collision costs into the cost function, the optimized B-spline curve control points can be obtained through optimization (e.g., using numerical optimization algorithms such as gradient descent or Newton's method). The B-spline curve generated from these control points is a smooth, safe, and traceable local obstacle avoidance path, such as... Figure 5 As shown.

[0068] V. Path Decision After executing the aforementioned global path planning and local path correction and obstacle avoidance planning, the system has obtained a safe driving path (which may be the original global path, the corrected path after translation correction, or a newly generated obstacle avoidance path). Based on the robot's current real-time positioning point, the decision module finds the nearest point on this safe driving path (i.e., the point on the path closest to the robot's current position), and then decides on the robot's tracking path for the current control cycle.

[0069] Specifically, the decision module performs a path decision once in each control cycle (e.g., 20 milliseconds to 50 milliseconds): it obtains the robot's current positioning coordinates (provided by GPS-RTK and IMU fusion positioning), searches for the point with the smallest Euclidean distance to the current position among all path points on the safe driving path as the nearest point, and then takes this nearest point as the starting point, extracts a fixed-length path segment (e.g., 5 to 10 meters) ahead as the path to be tracked in the current cycle, and transmits it to the tracking control module for tracking control.

[0070] In one embodiment, if the distance between the robot's current position and the nearest point on the safe driving path exceeds a preset threshold (e.g., 0.5 meters), it indicates that the robot may have deviated from the predetermined path for some reason (e.g., manual intervention or positioning change). At this time, the decision module will issue an alarm signal and replan the return path.

[0071] VI. Path Tracking Control After the decision module transmits the path to be tracked in the current cycle to the tracking control module, the tracking control module performs lateral tracking control and longitudinal speed control on the path and outputs control commands for the left and right track actuators.

[0072] 6.1 Lateral Tracking Control For low-speed automated operation scenarios in farmland, with algorithm robustness as the primary consideration, this invention adopts a pure tracking algorithm based on geometric relationships for path tracking.

[0073] like Figure 6 As shown, the principle of the pure tracking algorithm is as follows: Let ICR represent the robot's current instantaneous rotation center, R represent the rotation radius, dist represent the current period's aiming distance (i.e., the distance from the robot's current position to the aiming point on the track to be tracked), err represent the current moment's lateral error (i.e., the vertical distance from the robot's current position to the track to be tracked), and α represent the angle between the robot's current orientation angle and the aiming point direction.

[0074] according to Figure 6 The geometric relationships shown, the pure tracking geometric relationships can be expressed as: ; Lateral error (err) is defined as the deviation of the robot's current position from the aiming point in the lateral direction. Geometrically speaking: ; Combining the above two equations, the radius of rotation R can be further expressed as: ; Based on the differential kinematics characteristics of tracked robots, the formula for calculating the desired angular velocity can be obtained: ; Where, ω des The desired angular velocity (unit: radians / second), v des is the desired linear velocity (unit: m / s), err is the lateral error (unit: m), and dist is the aiming distance (unit: m).

[0075] In one embodiment, the pre-aiming distance `dist` is not a fixed value, but is dynamically adjusted according to the robot's current speed. The higher the speed, the longer the pre-aiming distance to ensure stability at high speeds; the lower the speed, the shorter the pre-aiming distance to ensure turning flexibility at low speeds. Specifically, a linear function can be used. dist = k·v des + dist min ; Dynamic calculations are performed, where k is the proportionality coefficient, and dist min This is the minimum aiming distance.

[0076] 6.2 Longitudinal speed control Due to the significant undulations in the orchard terrain, longitudinal speed control is crucial to ensure the robot's stability. Longitudinal control employs a closed-loop control strategy (in one embodiment, a PID control strategy), based on Newton's second law. The control quantity calculated by the closed-loop control is the robot's longitudinal acceleration, which is the motor driving force. During normal operation, closed-loop control is used to achieve closed-loop control between the desired and actual speeds of the robot.

[0077] Based on the aforementioned closed-loop control parameters, this invention incorporates slope compensation logic into the longitudinal control strategy to enhance the robot's maneuverability. Specifically, assuming the robot's pitch angle is denoted as pitch (obtained in real-time by the IMU), the slope compensation is g·sin(pitch), where g is the acceleration due to gravity (approximately 9.8 m / s²). 2 If the robot is climbing (pitch > 0), subtracting g·sin(pitch) from the speed closed-loop control variable will achieve the effect of slowing down and increasing torque, thereby enhancing the robot's climbing performance and preventing climbing failure or sudden speed drop due to insufficient power. If the robot is going downhill (pitch < 0), a corresponding compensation variable will be added to the control variable to prevent overspeed.

[0078] In addition, the decision-making module identifies turning points from the global path and passes this flag to the tracking control module. Upon receiving the current path to be tracked, the tracking control module executes deceleration logic as soon as it identifies a turning point, enabling the robot to complete the turning maneuver at a lower speed to reduce the risk of tipping over due to centrifugal force.

[0079] 6.3 Stationary Steering Control Mode Considering that in actual orchard environments, the elevation of the top of some plots is higher than the elevation between the working rows, if the robot turns using a double-circle tangent method, the excessive tilt angle under centripetal force may cause it to tip over. To address this special terrain and enhance driving safety, this invention incorporates a stationary turning control mode at the top of the plot.

[0080] Specifically, once the decision-making module identifies the end marker, the tracking control module first decelerates to zero (bringing the robot to a complete stop). Then, it uses a closed-loop control algorithm (in one embodiment, a PID control algorithm) to control the robot to gradually change its course from the inter-row heading to the target heading of the next work row, achieving a turn in place. After the turn in place is completed, the robot restarts along the direction of the next work row and accelerates to the desired working speed. The remaining path segments are controlled laterally using the aforementioned pure tracking algorithm.

[0081] The specific implementation of turning in place is as follows: the left and right track motors rotate in opposite directions (i.e., the left track moves forward and the right track moves backward, or vice versa), causing the robot to rotate around its own center, thereby changing the heading angle. During the rotation, the IMU provides real-time feedback on the robot's heading angle, and the closed-loop controller calculates the speed commands for the left and right tracks based on the deviation between the current heading angle and the target heading angle, until the heading angle reaches the target value.

[0082] 6.4 Differential Kinematics Conversion If the robot's underlying VCU lacks logic for converting linear velocity and angular velocity to the linear velocity of the left and right actuators, the tracking control module can directly incorporate this conversion logic. Let width represent the robot's wheelbase (i.e., the distance between the center lines of the left and right tracks, in meters), and ω... des and v des Let represent the desired angular velocity and desired linear velocity obtained from lateral control and longitudinal control, respectively. Then the desired linear velocities of the left and right tracks are respectively: ; ; Among them, v l v represents the desired linear velocity of the left track (in meters per second). r Let ω be the desired linear velocity of the right track (in meters per second). des When ω > 0, the robot turns to the right (the speed of the right track is greater than that of the left); when ω des When ω < 0, the robot turns to the left (the speed of the left track is greater than that of the right); when ω des When the speed is 0, the robot moves in a straight line (the speeds of the left and right tracks are equal).

[0083] Alternatively, the output of the final tracking control module can also be selected by directly selecting the execution speed of the left and right track motors (in revolutions per second or radians per second). In this case, the linear velocity needs to be converted into the motor speed based on the radius of the track drive wheel.

[0084] The tracking control module sends the calculated left and right track speed commands to the underlying VCU via CAN bus or serial communication. The VCU then drives the left and right track motors through the motor driver, thereby realizing a complete data closed loop for the navigation system.

[0085] VII. Preferred Embodiments In a preferred embodiment, the above steps are performed according to the following preferred parameters: The lidar uses a 16-line or 32-line mechanical lidar with a horizontal scanning angle range of 360°, a vertical scanning angle range of 30°, and an effective detection range of 100 meters. GPS-RTK has a positioning accuracy of centimeters (horizontal positioning error ≤ 2.5 cm + 1 ppm). The IMU has a pitch angle measurement accuracy of 0.1° and a heading angle measurement accuracy of 0.3°. The control cycle is 50 milliseconds; The interpolation interval for the global path is 0.2 meters; The lateral deviation threshold is 0.1 meters, and the heading deviation threshold is 5°. The dynamic range of the pre-aiming distance is 2 meters to 8 meters; Safety boundary distance d safe Calculated in real time based on vehicle width, safety redundancy, and positioning error; Smoothness weight ω smooth and collision weight ω collision Typical values ​​are: ω smooth =1.0, ω collision =5.0.

[0086] Under the above-mentioned preferred parameters, the tracked orchard plant protection robot of the present invention can achieve fully autonomous navigation operations in typical orchard environments, including automatic row following, automatic turning at the edge of the field, automatic obstacle avoidance or stopping, and adaptive speed control in sloping terrain, with high environmental adaptability and operational safety.

[0087] VIII. Application Scenarios Overview This embodiment uses a typical citrus orchard in a hilly area of ​​southern my country as an example to further illustrate the invention. The orchard is located on a gentle hilly slope with a gradient ranging from 5° to 20°, including transverse slopes, longitudinal slopes, and mixed slopes. The total area of ​​the orchard is approximately 15 mu (about 1 hectare), with a row spacing of 3.5 meters (artificial planting, actual row spacing fluctuates between 3.2 and 3.8 meters), a tree spacing of 2.5 meters, and the trees are 5 years old with a crown diameter of approximately 2.0 meters. The orchard plot is an irregular polygon, with an elevation rise of approximately 2 meters at the edge. There are static obstacles between the rows, such as 3 utility poles and 2 drainage posts. During the work season, agricultural tricycles and workers occasionally pass through. On the day of the work, the weather was sunny, the ground was soft dirt, and some areas were slightly slippery due to the previous day's rainfall.

[0088] 8.1 Preparations before the assignment At 8:00 a.m. on the day of the operation, the operator arrived at the orchard with the robot remote control. The operator first walked around the boundary of the orchard plot, and then pressed the record button at the four corner points and the boundary turning points of the plot with the remote control, collecting the GPS latitude and longitude coordinates of a total of 12 boundary points to form an electronic fence for the plot.

[0089] Subsequently, the operator entered the following parameters on the remote control's human-machine interface: row spacing of 3.5 meters, plant spacing of 2.5 meters, desired operating speed of 1.2 meters per second, turning speed at the edge of the field of 0.5 meters per second, and spray pressure of 1.2 MPa. The operator also selected the "Enable slope compensation" and "Enable in-situ turning mode" options on the remote control interface.

[0090] After the above data entry is completed, the remote controller packages the land information and operation parameters into a JSON format configuration file and sends it to the robot's industrial control computer via wireless communication. Upon receiving the configuration file, the industrial control computer parses it and converts the 12 electronic fence coordinate points from the WGS-84 coordinate system to the Northeast-Sky local coordinate system. After the conversion is complete, the operator activates the "One-Click Operation" button via the remote controller, and the robot begins its autonomous navigation operation.

[0091] 8.2 Global Path Planning After the industrial control computer parses the configuration file, the global path planning module is immediately started.

[0092] First, the module extracts six boundary points on each side perpendicular to the direction of the fruit tree rows from the 12 coordinate points of the electronic fence. Then, it performs cubic polynomial curve fitting on these boundary points to obtain smooth left and right boundary lines of the fence. These fitted boundary lines form an irregular polygonal region, which represents the actual workable area for the robot.

[0093] Then, based on the user-input row spacing of 3.5 meters, the module starts from the boundary of the fence closest to the starting point of the fruit tree row and advances inwards in a direction perpendicular to the fruit tree row, determining the position of a work row every 3.5 meters. After calculation, a total of 24 work rows are planned for this 15-acre plot. Taking the midpoint of the coordinates of two adjacent fruit tree rows, 23 intermediate work paths (i.e., the actual driving paths of the robot) are obtained. Each work path consists of a series of path points, with an adjacent path point spacing of 0.2 meters.

[0094] Next, the module generates the turning path between each pair of adjacent work rows using the aforementioned double-circle internal tangent method, based on the coordinates of the start and end points of adjacent work rows. Taking one turn as an example: point A is the end of the previous work row, point B is the start of the next work row, the row spacing is d = 3.5 meters, and the user sets θ = 60°. Following the aforementioned geometric method, the system first draws a perpendicular line upwards from point A and rotates it 60° clockwise to obtain ray lc. lc intersects the perpendicular line downwards from point B at orB, and rB is measured to be 2.9 meters. Then, according to the internal tangent equation |orA|... orB|=|rA rB| solves for rA = 1.5 meters, and orA is located 1.5 meters directly above point A; finally, the position of the tangent point is calculated. From point A, travel along the corresponding arc to the tangent, then along the corresponding arc to point B, thus generating the complete turning path. The module performs the above calculations for each of the 22 bends along the entire route, generating their respective U-turn paths.

[0095] Finally, the module interpolates and densifies the 23 straight-line operation paths and 22 circular turning paths at the ends of the fields (interpolation interval of 0.2 meters), and then splices them together sequentially according to the driving order to form a complete global path. The total length of this global path is approximately 5200 meters, covering all rows of fruit trees within the entire electronic fence. After the global path is generated, the module stores it in the memory of the industrial control computer and awaits real-time correction from the sensing module.

[0096] 8.3 Robot Start-up and Entry At 8:20 a.m., after confirming that the area around the robot was safe, the operator sent a start command via remote control. The robot started from the beginning position at the edge of the plot and entered the global path along the first work row.

[0097] Once the robot entered the work area, the perception module immediately began operating. A 16-line LiDAR continuously scanned the surrounding environment at a frequency of 10Hz, acquiring point cloud data of the fruit trees on both sides. The perception algorithm on the industrial control computer performed DBSCAN clustering on the point cloud (cluster radius set to 0.3 meters, minimum cluster size set to 5 points), successfully identifying the positions of the fruit tree rows on the left and right sides. The midpoint of each fruit tree row was then used to fit the centerline of the current working row.

[0098] The GPS-RTK and IMU fusion positioning module outputs the robot's current positioning coordinates and attitude information (including position, heading angle, pitch angle, and roll angle) in real time, with an update frequency of 20Hz. The decision module compares the robot's current position with the work line centerline fitted by the perception module, and calculates the current lateral deviation as 0.25 meters (robot is leaning to the left) and heading deviation as -3.5° (robot is facing to the right).

[0099] Since the lateral deviation of 0.25 meters exceeded the preset threshold of 0.1 meters, the correction module triggered the first path translation operation. Specifically, the correction module translated the global path 0.25 meters to the right and performed a -3.5° heading correction. The corrected path was sent to the tracking control module via the CAN bus, and the robot began to move from its current position toward the corrected path. After an online process of approximately 5 seconds, the robot successfully moved onto the corrected path.

[0100] Subsequently, the robot continuously monitored lateral and directional deviations. During the movement of the first work row, the lateral deviation remained between -0.08 meters and +0.12 meters, and the directional deviation remained between -3° and +4°. Whenever the deviation exceeded the threshold, the correction module triggered a new round of translational correction. During the entire movement of the first work row, which was approximately 220 meters long, a total of 6 translational corrections were triggered, with a maximum translation of 0.18 meters, effectively compensating for the row spacing deviations caused by manual planting.

[0101] 8.4 Lane Driving and Obstacle Avoidance When the robot had traveled approximately 80 meters along the first work row, the LiDAR in the perception module detected an abnormal obstacle 15 meters in front of the robot. After point cloud clustering and shape recognition, the obstacle was determined to be a utility pole in the orchard, approximately 0.3 meters in diameter, located about 0.8 meters to the right of the work row's centerline. The obstacle detection algorithm then marked the obstacle's location on a local grid map (map size 20 meters × 20 meters, grid resolution 0.05 meters).

[0102] Meanwhile, the grid map module analyzes the drivable area around the utility pole. The analysis results show that there is a drivable passage of about 1.6 meters wide on the left side of the utility pole (the robot is 1.2 meters wide, with a safety redundancy of 0.3 meters and a minimum passage width requirement of 1.5 meters). Therefore, it is determined that the drivable area meets the safe passage conditions.

[0103] The obstacle avoidance planning module is then activated. First, the A search algorithm searches for the shortest path from the robot's current position to the recovery point on the grid map. The searched path detours to the left at the utility pole location. Then, the module samples six points from path A as initial control points for a cubic uniform B-spline curve and optimizes these control points. In this embodiment, the smoothness weight ω... smooth Set to 1.0, collision weight ω collision Set to 5.0 (increasing collision weight to ensure safety due to the proximity of the obstacle), safety boundary d safe Set to 1.0 meter, danger inner boundary d max The distance was set to 0.8 meters. After optimization, a smooth obstacle avoidance path was obtained. This path gradually shifts to the left by about 0.7 meters from the original work row centerline, bypasses the utility pole, and then smoothly returns to the original work row centerline. The total length of the obstacle avoidance path is about 8 meters, and the maximum radius of curvature is 2.5 meters, which is well within the feasible range of robot kinematics.

[0104] The decision-making module sends the generated obstacle avoidance path as the current tracking path to the tracking control module. The robot travels along the obstacle avoidance path, successfully bypasses the utility pole, and returns to its original work row without any collisions or sudden stops. The entire process, from obstacle detection to obstacle avoidance, takes approximately 6 seconds.

[0105] During subsequent travel, the robot detected another water outlet obstacle approximately 50 meters into the third work row. However, the analysis of the drivable area showed that the water outlet was almost in the center of the row, with less than 1.2 meters of passable width on both sides, making safe passage impossible. Based on this, the obstacle avoidance planning module determined that a safe obstacle avoidance path could not be generated, and the decision module issued an emergency stop command. The robot smoothly braked to a stop and issued an audible and visual alarm to the operator via the remote control interface: "Obstacle ahead, impassable, manual intervention required." After the operator arrived and removed the water outlet, they sent a "continue work" command via the remote control, and the robot restarted from its stopped position and continued to complete the subsequent work.

[0106] 8.5 Turning around at the edge of the field and turning in place After completing the first work row of approximately 220 meters, the robot reaches the edge of the field. At this point, the robot's position is close to the edge marker, and the point cloud features of the fruit tree rows detected by the perception module gradually become sparse (because there are no fruit trees in the edge area). The reliability of the lateral deviation and heading deviation data decreases, and the correction module automatically shuts down.

[0107] After the tracking control module identifies the boundary marker, it first executes the deceleration logic. The robot linearly decelerates from its current operating speed of 1.2 m / s to a turning speed of 0.5 m / s within 3 seconds. However, due to an elevation rise of approximately 2 meters at the boundary of the site, the decision module determines that turning using the conventional double-circle inward turning method carries a risk of tipping over, thus triggering the in-situ turning control mode.

[0108] In stationary turning mode, the tracking control module continues to decelerate, further reducing the robot's speed to 0 m / s (complete stop) within 2 seconds. Subsequently, the left and right track motors rotate in opposite directions (left track forward, right track backward), causing the robot to rotate in place around its center. The IMU provides real-time feedback on the heading angle change, and the PID controller (proportional coefficient K)... p =1.2, integral coefficient K i =0.05, differential coefficient K d =0.02) The speed difference between the left and right tracks is adjusted in real time based on the deviation between the current heading angle and the target heading angle (the direction of the second working line). After about 8 seconds, the robot's heading angle rotates 175° from its original direction (slightly less than 180° to compensate for the positioning error), successfully aligning with the direction of the second working line.

[0109] After the heading is aligned, the robot restarts, with its left and right tracks accelerating synchronously to 0.5 m / s (turning speed at the edge of the field). Upon entering the second work row, it accelerates again to the desired working speed of 1.2 m / s. After the perception module reacquires the point cloud information of the fruit tree rows on both sides, the lateral deviation and heading deviation data become valid again, and the correction module is reactivated to continue real-time path correction.

[0110] 8.6 Adaptive Speed ​​Control for Ramps When the robot entered the fifth work row, there was an uphill section of about 30 meters long with a slope of about 15°. The IMU detected that the robot's pitch angle gradually increased to about 14.8°, and the slope compensation module then intervened.

[0111] Longitudinal speed control uses a PID controller (proportional coefficient K) p =0.8, integral coefficient K i =0.1, differential coefficient K d =0.03) to perform closed-loop control between the desired speed (1.2 m / s) and the actual speed. The acceleration control value calculated by the PID controller is approximately 0.15 m / s. 2 Meanwhile, the slope compensation module calculates the slope compensation amount as g·sin(pitch) = 9.8 × sin(14.8°) ≈ 2.5 m / s. 2 Subtracting this compensation amount (due to the ramping state) from the PID control quantity significantly increases the actual driving force.

[0112] Under the slope compensation logic, the robot's actual speed gradually decreased from 1.2 m / s to approximately 0.85 m / s, and stabilized at this speed level to complete the climb. Although the speed decreased, the motor torque was significantly increased, allowing the robot to smoothly traverse the entire uphill section with sufficient power, without any sudden speed drops, slippage, or failure to climb. Once the robot passed the crest of the hill and the pitch angle returned to zero, the slope compensation gradually decreased to 0, and the robot automatically accelerated back to the desired operating speed of 1.2 m / s.

[0113] In the subsequent 12th work row, the robot encountered a downhill section of approximately 20 meters (slope of approximately -12°), and the IMU detected a pitch angle of approximately -11.5°. The slope compensation module calculated the compensation amount as g·sin(pitch) = 9.8 × sin(-11.5°) ≈ -1.95 m / s. 2 By subtracting this negative compensation amount from the PID control input (i.e., actually adding a compensation of 1.95 m / s²), speed loss due to gravity acceleration during downhill driving was prevented. The robot's actual speed remained stable within 1.3 m / s, safely completing the downhill journey.

[0114] 8.7 Cross-industry operations and full-process coverage In subsequent operations, the robot completed rows 2 through 24 sequentially. After completing each row, it turned around at the edge of the field using either the in-situ turn or the double-circle inward turning method to enter the next row. For the plots where the elevation of the edge of the field was basically the same as that between the rows (a total of 4 edge locations), the system used the double-circle inward turning method. The robot smoothly turned along the double-circle path at a speed of 0.5 m / s. During the turn, the deceleration logic remained active, and the centrifugal force was controlled within a safe range.

[0115] The robot detected a dynamic obstacle approximately 150 meters into its 8th work row: a farm tricycle crossing laterally between rows. The perception module identified the obstacle as dynamic (its point cloud position was continuously changing) and determined its trajectory would traverse the robot's path. Since the obstacle left the robot's path within 2 seconds, the system determined that no obstacle avoidance path needed to be generated. Instead, it issued a temporary deceleration command, reducing the speed from 1.2 m / s to 0.3 m / s. Once the tricycle had completely left and the drivable area ahead was clear, the robot automatically accelerated back to 1.2 m / s and continued working. No collisions or emergency stops occurred throughout the entire process, ensuring the continuity of operations.

[0116] The entire operation lasted approximately 3 hours (including obstacle avoidance, turning in place, and temporary deceleration time). The robot traveled a total distance of approximately 5200 meters and completed plant protection spraying operations on 24 rows of fruit trees, covering the entire 15-acre orchard. During the operation, the spraying system operated continuously and stably according to the preset spraying pressure of 1.2 MPa, with a pumping flow rate of approximately 8 liters per minute. The liquid tank has a capacity of 200 liters, and each tank of liquid can cover approximately 6 acres of orchard. The robot stopped once between rows to replenish the liquid during the operation (replenishment took approximately 5 minutes).

[0117] 8.8 Homework completed At 11:20 AM, after the robot completed its final work line and arrived at the end point of the field, the decision-making module determined that the entire path had been completed and automatically issued a stop command. The robot decelerated to 0, the left and right track motors braked and locked, and the spray system automatically shut off.

[0118] Subsequently, the robot notified the operator of the "Operation Complete" message through both the remote control interface and onboard voice prompts. At the same time, it uploaded the statistical data of this operation (including a total driving distance of approximately 5.2 kilometers, an operation time of approximately 3 hours, a cumulative spraying area of ​​approximately 15 acres, 2 obstacle avoidances, 0 emergency stops, and an average operating speed of approximately 0.96 meters per second) to the remote control for the operator to view and save.

[0119] After the operator arrives at the robot's location, they send a "shutdown" command via remote control. The robot completes its system self-check and shuts down safely. At this point, the plant protection operation for the orchard is complete. The entire process requires no manual driving or continuous intervention; the operator only needs to perform site marking and parameter configuration before the operation, and handle one instance of obstruction by the water outlet.

[0120] The above description is merely an embodiment of this application and is not intended to limit this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principle of this application should be included within the scope of the claims of this application.

Claims

1. An autonomous navigation method for a tracked orchard plant protection robot, characterized in that, include: Acquire and parse plot information, which includes row spacing and plant spacing of fruit tree rows, coordinate point information of electronic fence, expected operation speed and turning speed at the edge of the field; Based on the electronic fence and the row spacing of the fruit tree rows, inter-row path planning is performed within the electronic fence, and a double-circle in-circle method is used to generate a field-end turning path. The inter-row path and the field-end turning path are then spliced ​​together to form a global path covering all fruit tree rows. During the robot's journey along the global path, the position information of the fruit tree rows identified by the perception module is acquired in real time, the lateral deviation and heading deviation are calculated, and the global path is corrected based on the lateral deviation and heading deviation; when an obstacle is detected, the drivable area information is acquired, and an obstacle avoidance path is generated or a stop command is issued based on the drivable area information. The robot's real-time location points determine the path to be followed from the current safe driving path; Lateral tracking control and longitudinal speed control are performed on the path to be tracked, and control commands are output to the left and right track actuators. The lateral tracking control is based on the geometric relationship of the pre-aiming point and calculates the desired angular velocity according to the pre-aiming distance and lateral error. The longitudinal speed control adopts a closed-loop control strategy, superimposing a slope compensation amount based on the pitch angle on the control quantity to achieve climbing speed reduction and torque increase, and decelerating to the turning speed when the turning point is detected.

2. The autonomous navigation method for a tracked orchard plant protection robot according to claim 1, characterized in that, The step of concatenating the inter-row path with the field-end turning path to form a global path covering all fruit tree rows specifically includes: Based on the electronic fence, fence points perpendicular to both sides of the fruit tree row are extracted, and polynomial curve fitting is performed on the fence points to obtain fence boundary information; Based on the row spacing of the fruit tree rows, the plots within the electronic fence are scanned row by row to obtain the coordinate point information of each fruit tree row. The midpoint of the coordinate points of adjacent fruit tree rows is taken to obtain the intermediate work point information of adjacent fruit tree rows. Based on the start and end point pair information of each work row, the turning-around path between adjacent work rows is generated using the double-circle in-circle method; The global path is generated by concatenating the inter-row paths of each work row with the turning-off path at the edge of the field after interpolation and encryption.

3. The autonomous navigation method for a tracked orchard plant protection robot according to claim 1, characterized in that, The step of correcting the global path based on the lateral deviation and heading deviation includes: After the robot enters the course, it travels along the global path and obtains the lateral deviation and heading deviation output by the perception module in real time, and removes noise and outliers in the lateral deviation and heading deviation. Based on the removed lateral and heading deviations, the global path is translated to move the robot from its current position toward the translated path. Continuously monitor lateral and heading deviations, and repeat the translation operation when the deviation exceeds a preset threshold. When the robot reaches the starting point marker, it turns off the correction function and generates a turning path according to the double-circle in-circle method to complete the turning.

4. The autonomous navigation method for a tracked orchard plant protection robot according to claim 1, characterized in that, The step of generating an obstacle avoidance path based on the drivable area information includes: From A Multiple points are sampled on the path generated by the search algorithm and used as the initial control points of the cubic uniform B-spline curve. A cost function is constructed, which includes a smoothness cost and a collision cost. The optimized control points are obtained by optimizing the cost function, and the obstacle avoidance path is generated from the optimized control points. The smoothness cost is characterized by elastic band energy: ； Among them, Q i Let n be the i-th control point, and n be the total number of control points. The collision cost is represented by a potential field, and the single-point collision cost is: ； Where d(p) is the distance from point p to the nearest obstacle, d safe For the safety boundary distance, d max This represents the distance to the inner boundary of the danger zone.

5. The autonomous navigation method for a tracked orchard plant protection robot according to claim 4, characterized in that, The cost function is: ； ω smooth For smoothness weights, ω collision For collision weights, f collision (Q) is the sum of the single-point collision costs of the four boundary points after modeling the robot as a rectangle.

6. The autonomous navigation method for a tracked orchard plant protection robot according to claim 1, characterized in that, In the lateral tracking control, the desired angular velocity is calculated according to the following formula: ； Where, ω des For the desired angular velocity, v des Here, is the desired linear velocity, err is the lateral error, and dist is the aiming distance.

7. The autonomous navigation method for a tracked orchard plant protection robot according to claim 1, characterized in that, In the longitudinal speed control, the slope compensation amount is: g·sin(pitch); Where g is the acceleration due to gravity, pitch is the pitch angle, and when the robot is climbing, the slope compensation is subtracted from the control quantity to achieve speed reduction and torque increase.

8. The autonomous navigation method for a tracked orchard plant protection robot according to claim 1, characterized in that, The steps of performing lateral tracking control and longitudinal speed control on the path to be tracked also include an in-situ steering control mode: When a ground marker is detected, the robot decelerates to zero and then uses a closed-loop control algorithm to control the robot's heading angle to achieve a turn in place.

9. The autonomous navigation method for a tracked orchard plant protection robot according to claim 1, characterized in that, In the step of acquiring and parsing land parcel information, the coordinate point information of the electronic fence is converted from the 84 coordinate system to the Northeast-Sky local coordinate system, and the global path is generated under the Northeast-Sky local coordinate system.

10. An autonomous navigation system for a tracked orchard plant protection robot, characterized in that, include: The information configuration and parsing module is used to acquire and parse plot information, which includes the row spacing and plant spacing of fruit tree rows, the coordinate point information of the electronic fence, the expected operation speed, and the turning speed at the edge of the field. The global path planning module is used to generate a global path based on the electronic fence and row spacing. The global path includes the inter-row path and the turning path generated by the double circle incision method. The perception module is used to identify the position of the fruit tree rows and obstacle information in real time, and output the lateral deviation and heading deviation. The local path correction and obstacle avoidance planning module is used to translate and correct the global path based on the lateral deviation and heading deviation, and to generate an obstacle avoidance path or stop the obstacle when encountering an obstacle. The decision-making module is used to determine the path to be tracked based on the real-time location points from the current driving path; The tracking control module is used to perform lateral tracking and longitudinal speed control on the path to be tracked, and output left and right track commands; the lateral tracking calculates the desired angular velocity based on the pre-aiming distance and lateral error; the longitudinal speed control adopts a closed-loop strategy, superimposed with pitch angle and slope compensation to achieve speed reduction and torque increase when climbing, and decelerates when turning at the edge of the terrain.

Images