Orchard in-row navigation method and system

Abstract

The application relates to an orchard in-row navigation method and system for an autonomous navigation robot with a laser radar in a hedge orchard, the navigation method comprising the following steps: taking the position of the robot with the laser radar as an origin, constructing a three-dimensional coordinate space comprising an x-axis, a y-axis and a z-axis, and scanning real objects on both sides of the robot in real time to obtain original point cloud data corresponding to the real objects; dividing the three-dimensional coordinate space into multiple cubic bodies of equal size, randomly selecting a coordinate point from the original point cloud data in each cubic body to construct standard point cloud data; and generating navigation reference line parameters through multiple parallel line fitting according to the standard point cloud data, the navigation reference line parameters being used to generate a reference track for autonomous navigation of the robot in the hedge orchard row. The application processes the point cloud data through parallel line fitting to obtain the navigation reference line parameters, so as to control the robot, and solves the problems of poor navigation stability and low precision when the positioning signal is blocked or the coverage range is insufficient.

Description

Technical Field

[0001] This application relates to the field of navigation, and in particular to a method and system for navigation within orchard rows. Background Technology

[0002] As the scale of orchard production continues to expand, traditional manual management and operation methods are no longer sufficient to meet production demands. However, orchard navigation technology can guide machinery to operate according to preset paths and parameters, ensuring the accuracy and quality of the work.

[0003] Currently, autonomous navigation of robots in trellis-style orchards primarily relies on real-time dynamic differential positioning technology, which requires coverage of the orchard environment. By pre-planning a global navigation satellite system trajectory, the robot, after starting, uses tracking algorithms to follow the path, thereby achieving autonomous navigation.

[0004] Currently, the navigation accuracy of related technologies mainly depends on the coverage and signal strength of real-time dynamic differential positioning technology. When the coverage is insufficient or the signal is blocked, the signal is distorted, resulting in poor navigation stability and low accuracy. Summary of the Invention

[0005] This application provides a method and system for orchard row navigation, which at least solves the problems of poor navigation stability and low accuracy in related technologies when the positioning signal is blocked or the positioning coverage is insufficient.

[0006] In a first aspect, embodiments of this application provide an orchard row navigation method for an autonomous navigation robot equipped with LiDAR in a trellis orchard. The navigation method includes:

[0007] The point cloud data acquisition step involves taking the location of the robot equipped with LiDAR as the origin and constructing a three-dimensional coordinate space including the x-axis, y-axis, and z-axis. The robot scans the objects on both sides in real time to obtain the original point cloud data corresponding to the objects.

[0008] The point cloud data processing steps involve dividing the three-dimensional coordinate space into multiple cubes of equal size, and randomly selecting a coordinate point from the original point cloud data in each cube to form standard point cloud data.

[0009] The navigation baseline parameter generation step involves selecting coordinate points from standard point cloud data based on parallel line fitting for calculating navigation baseline parameters; and generating navigation baseline parameters based on the selected coordinate points. These navigation baseline parameters are used to generate a reference trajectory for the robot's autonomous navigation within the trellis orchard rows.

[0010] In some embodiments, the navigation reference parameter generation step further includes:

[0011] The initial division steps are as follows: each coordinate point in the standard point cloud data includes x, y and z values. The coordinate points with y values ​​greater than zero in the standard point cloud data are selected and recorded as the first side standard point cloud set, and the coordinate points with y values ​​less than zero in the standard point cloud data are selected and recorded as the second side standard point cloud set.

[0012] The preliminary fitting step involves fitting parallel lines based on the coordinates of the first and second standard point cloud sets, calculating the preliminary slope, preliminary left intercept, and preliminary right intercept; generating the first-side parallel line based on the preliminary slope and preliminary left intercept; and generating the second-side parallel line based on the preliminary slope and preliminary right intercept.

[0013] Wherein, the initial slope is the slope of the first parallel line and the slope of the second parallel line, the initial left intercept is the y-intercept of the first parallel line, and the initial right intercept is the y-intercept of the second parallel line.

[0014] The precise division steps are as follows: calculate the distance from each coordinate point in the first-side standard point cloud set to the first-side parallel line; calculate the distance from each coordinate point in the second-side standard point cloud set to the second-side parallel line; set a first distance threshold; select coordinate points in the first-side standard point cloud set whose distance values ​​are less than the first distance threshold to generate the first-side precise point cloud set; select coordinate points in the second-side standard point cloud set whose distance values ​​are less than the first distance threshold to generate the second-side precise point cloud set.

[0015] The precise fitting step involves calculating the navigation baseline parameters by fitting parallel lines based on the first and second side precise point cloud sets, and then sending the navigation baseline parameters to the robot.

[0016] The navigation baseline parameters include precise slope, precise left intercept, and precise right intercept. The precise slope is the slope of the straight line formed by the precise fitting of the first side precise point cloud set and the slope of the straight line formed by the precise fitting of the second side precise point cloud set. The precise left intercept is the intercept of the straight line formed by the precise fitting of the first side precise point cloud set on the y-axis. The precise right intercept is the intercept of the straight line formed by the precise fitting of the second side precise point cloud set on the y-axis.

[0017] In some embodiments, the precise fitting step further includes:

[0018] The formula for calculating the precise slope using parallel line fitting is:

[0019] ;

[0020] in, For precise slope, This is the sum of the y-values ​​of all coordinate points in the first side's precise point cloud set. For accurate left intercept, The number of coordinate points in the first side's precise point cloud set. It is the sum of the x-values ​​of all coordinate points in the first side of the precise point cloud set.

[0021] In some embodiments, the precise fitting step further includes:

[0022] The formula for calculating the precise left intercept using parallel line fitting is as follows:

[0023] ;

[0024] in, For accurate left intercept, This is the sum of the y-values ​​of all coordinate points in the first side's precise point cloud set. This is the sum of the y-values ​​of all coordinate points in the second-side precise point cloud set. This is the sum of the squares of the x-values ​​of each coordinate point in the first and second side precise point cloud sets. It is the sum of the products of the x and y values ​​of each coordinate point in the first and second side precise point cloud sets. This is the sum of the x-values ​​of all coordinate points in the first side's precise point cloud set. This is the sum of the x-values ​​of all coordinate points in the second side's precise point cloud set. The number of coordinate points in the first side's precise point cloud set. This represents the number of coordinate points in the second side's precise point cloud set.

[0025] In some embodiments, the precise fitting step further includes:

[0026] The formula for calculating the precise right intercept using parallel line fitting is as follows:

[0027] ;

[0028] in, To achieve accurate right intercept, This is the sum of the y-values ​​of all coordinate points in the second-side precise point cloud set. For precise slope, This is the sum of the x-values ​​of all coordinate points in the second-side precise point cloud set. This represents the number of coordinate points in the second side's precise point cloud set.

[0029] In some embodiments, the navigation method further includes:

[0030] The ground detection step involves performing ground detection on the standard point cloud data. If no ground is detected, invalid navigation parameters and invalid identifiers are output; if ground is detected, valid identifiers are output.

[0031] The system controls the robot to operate within the trellis-type orchard based on valid markers and navigation baseline parameters. Invalid markers indicate that the robot does not use invalid navigation parameters, while valid markers indicate that the robot uses navigation baseline parameters.

[0032] In some embodiments, the navigation method further includes:

[0033] The ground detection step involves setting the detection range and a second distance threshold, comparing the x-values ​​in the standard point cloud data within the detection range to obtain the maximum x-value, and determining whether the maximum x-value is less than the second distance threshold. If so, the robot is controlled to stop or turn around.

[0034] Secondly, embodiments of this application provide an orchard row navigation system for an autonomous navigation robot equipped with LiDAR in a trellis orchard. The navigation system includes:

[0035] The point cloud data acquisition module is configured to take the location of the robot with LiDAR as the origin and form a three-dimensional coordinate space including the x-axis, y-axis and z-axis. It scans the objects on both sides of the robot in real time and acquires the original point cloud data corresponding to the objects.

[0036] The point cloud data processing module, connected to the point cloud data processing module, is configured to acquire three-dimensional coordinate space and raw point cloud data, divide the three-dimensional coordinate space into multiple cubes of equal size, and randomly select a coordinate point from the raw point cloud data in each cube to form standard point cloud data.

[0037] The navigation reference parameter generation module, connected to the point cloud data processing module, is configured to acquire standard point cloud data, filter coordinate points from the standard point cloud data based on parallel line fitting for calculating navigation reference line parameters, and generate navigation reference line parameters based on the selected coordinate points. The navigation reference line parameters are used to generate a reference trajectory for the robot's autonomous navigation within the trellis-type orchard rows.

[0038] In some embodiments, the navigation system further includes:

[0039] The ground detection module connects the point cloud data processing module and the robot. It is configured to perform ground detection on standard point cloud data. If no ground is detected, it outputs invalid navigation parameters and invalid identifiers to the robot; if ground is detected, it outputs valid identifiers to the robot.

[0040] The system controls the robot to operate within the trellis-type orchard based on valid markers and navigation baseline parameters. Invalid markers indicate that the robot does not use invalid navigation parameters, while valid markers indicate that the robot uses navigation baseline parameters.

[0041] In some embodiments, the navigation system further includes:

[0042] The ground detection module connects the point cloud data processing module and the robot. It is configured to set the detection range and the second distance threshold, compare the x-values ​​in the standard point cloud data within the detection range, obtain the maximum x-value, and determine whether the maximum x-value is less than the second distance threshold. If so, it controls the robot to stop or turn around.

[0043] Compared to related technologies, the orchard row navigation method and system provided in this application, by processing point cloud data reflecting the actual location of objects, obtains navigation baseline parameters used by the navigation robot, solving the problems of poor navigation stability and low accuracy when the positioning signal is blocked or the positioning coverage is insufficient. It achieves navigation stability and reliability without relying on real-time dynamic differential positioning, has a wide range of applications, and effectively copes with complex and ever-changing orchard environments.

[0044] Details of one or more embodiments of this application are set forth in the following drawings and description to make other features, objects and advantages of this application more readily apparent. Attached Figure Description

[0045] The accompanying drawings, which are included to provide a further understanding of this application and form part of this application, illustrate exemplary embodiments and are used to explain this application, but do not constitute an undue limitation of this application. In the drawings:

[0046] Figure 1 This is a flowchart of an orchard row navigation method according to an embodiment of this application;

[0047] Figure 2 This is a flowchart of an orchard row navigation method according to an embodiment of this application;

[0048] Figure 3 This is a structural block diagram of an orchard row navigation system according to an embodiment of this application;

[0049] Figure 4 This is a flowchart of an orchard row navigation method according to an embodiment of this application;

[0050] Figure 5 This is a flowchart of an orchard row navigation method according to an embodiment of this application;

[0051] Figure 6 This is a flowchart of an orchard row navigation method according to an embodiment of this application.

[0052] In the picture:

[0053] 301. Point cloud data acquisition module; 302. Point cloud data processing module; 303. Navigation reference parameter generation module. Detailed Implementation

[0054] To make the objectives, technical solutions, and advantages of this application clearer, the application is described and illustrated below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application. All other embodiments obtained by those skilled in the art based on the embodiments provided in this application without inventive effort are within the scope of protection of this application.

[0055] Obviously, the accompanying drawings described below are merely some examples or embodiments of this application. Those skilled in the art can apply this application to other similar scenarios based on these drawings without any inventive effort. Furthermore, it is understood that although the efforts made in this development process may be complex and lengthy, for those skilled in the art related to the content disclosed in this application, any changes to design, manufacturing, or production based on the technical content disclosed in this application are merely conventional technical means and should not be construed as insufficient disclosure of the content of this application.

[0056] In this application, the reference to "embodiment" means that a specific feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places in the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment that is mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described in this application may be combined with other embodiments without conflict.

[0057] Unless otherwise defined, the technical or scientific terms used in this application shall have the ordinary meaning understood by one of ordinary skill in the art to which this application pertains. The terms “a,” “an,” “an,” “the,” and similar words used in this application do not indicate quantity limitation and may indicate singular or plural. The terms “comprising,” “including,” “having,” and any variations thereof used in this application are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or device that includes a series of steps or modules (units) is not limited to the listed steps or units, but may also include steps or units not listed, or may include other steps or units inherent to these processes, methods, products, or devices. The terms “connected,” “linked,” “coupled,” and similar words used in this application are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. “Multiple” used in this application refers to two or more. “And / or” describes the relationship between related objects, indicating that three relationships may exist; for example, “A and / or B” can represent: A alone, A and B simultaneously, and B alone. The character " / " generally indicates that the preceding and following objects are in an "or" relationship. The terms "first," "second," and "third" used in this application are merely to distinguish similar objects and do not represent a specific ordering of the objects.

[0058] In current trellis-style orchard operations, the core technology supporting autonomous navigation for robots is primarily Real-Time Kinematic (RTK) positioning. Because these orchards employ trellis cultivation, the fruit trees are neatly arranged in rows and columns along fixed supports, with relatively standardized row and plant spacing, forming a regular linear spatial structure. This provides a suitable environment for the application of RTK technology.

[0059] In terms of RTK coverage, its applicability in trellis orchards is closely related to the orchard's openness and surrounding obstructions. In flat trellis orchards where the trees have not yet reached their peak fruiting period, the tree canopy is not yet fully closed. The differential signal emitted by the RTK base station can penetrate the gaps in the sparse branches and leaves, forming stable communication with the Global Navigation Satellite System (GNSS) receiver on the robot. The positioning accuracy can reach the centimeter level, which is sufficient to meet the basic needs of the robot's operation along its path.

[0060] However, when the orchard is in its peak fruiting period, the dense foliage intertwines to form closed or semi-closed pathways, significantly exacerbating signal obstruction. This leads to fluctuations in RTK positioning stability and even signal loss in localized areas. Furthermore, the presence of tall trees, mountains, or buildings around the orchard can enhance RTK signal reflection and multipath effects, further compressing the effective coverage area.

[0061] To address the aforementioned issues, this application proposes an orchard row navigation method and system that does not rely on RTK signal coverage, enabling its application in remote areas with limited RTK signal coverage. This improves navigation accuracy and stability, eliminates the need for manual pre-planning of robot trajectories, and saves labor costs.

[0062] The parallel line fitting method described in this application is a special type of linear fitting method, commonly used in biological, economic, and engineering fields. Its core principle is to fit straight lines to multiple sets of data, ensuring all lines have the same slope and differ only in their intercepts. This method is often used for modeling data that exhibit common trends but differ in initial conditions or baselines, and it can more accurately capture the inherent patterns in the data.

[0063] This application provides an orchard row navigation method for an autonomous navigation robot equipped with LiDAR in a trellis orchard. Figure 1 As shown, the navigation methods include:

[0064] In step S101, point cloud data acquisition involves taking the location of the robot equipped with LiDAR as the origin and constructing a three-dimensional coordinate space including the x-axis, y-axis, and z-axis. The robot scans the objects on both sides in real time to acquire the original point cloud data corresponding to the objects.

[0065] Furthermore, when constructing the three-dimensional coordinate space, the origin is taken as the center position of the robot equipped with LiDAR.

[0066] The objects on either side of the robot include trellises and fruit trees.

[0067] In a trellis-style orchard, the robot is initialized between any two adjacent rows of fruit trees. The lidar is positioned in the middle of the front of the robot. The lidar scans the entities such as the trellis, fruit trees, and wires on both sides of the robot, obtaining their corresponding raw point cloud data in real time.

[0068] A three-dimensional coordinate system is established with the location of the robot equipped with LiDAR as the origin. This system includes the x-axis, y-axis, and z-axis. In this system, the positive x-axis represents the robot's forward direction, the positive y-axis represents the robot's leftward extension direction, and the positive z-axis represents the robot's upward extension direction.

[0069] The data for each coordinate point in the original point cloud data is the three-dimensional coordinate in a three-dimensional coordinate system, including the x, y and z values.

[0070] Based on the ridge width between rows of fruit trees and the height of the fruit trees in the actual orchard, a filtering range is set along the x-axis, y-axis and z-axis according to the three-dimensional coordinate system. After the lidar scan, the point cloud data within the filtering range is retained as the original point cloud data.

[0071] For example, the width of the ridges between fruit tree rows is 2.8 meters, and the height of the fruit trees is 3 meters.

[0072] The filtering range in the x-axis direction is set to [0, L] to obtain point cloud data within L meters in front of the lidar. Here, L>0 can be set according to the farthest distance that the lidar can scan, and the value range of L is [20, 30].

[0073] Set the filtering range in the y-axis direction to [-(2.8+m), 2.8+m] to obtain point cloud data at distances of 2.8+m on both sides of the lidar, where m>0 and can be 0.5.

[0074] The filtering range in the z-axis direction is set to [h, 3] to obtain point cloud data between h meters and 3 meters away from the lidar. Here, h>0 means that the point cloud data in the z-axis direction is higher than the ground, and the value of h ranges from [0.5, 1].

[0075] In point cloud data processing step S102, the three-dimensional coordinate space is divided into multiple cubes of equal size, and a coordinate point is randomly selected from the original point cloud data in each cube to form standard point cloud data.

[0076] Voxel downsampling is performed on the original point cloud data. Alternatively, the size of the cube can be set by the user. The three-dimensional coordinate space is divided according to the set size. The original point cloud data in the cube includes multiple coordinate points. Only one coordinate point is retained in each cube. This preserves the overall structure of the point cloud distribution and reduces the number of coordinate points in the original point cloud data, thereby reducing the computational complexity of subsequent navigation line fitting.

[0077] In step S103, the navigation reference parameter generation involves selecting coordinate points from standard point cloud data based on parallel line fitting for calculating navigation reference line parameters. Based on the selected coordinate points, navigation reference line parameters are generated. These parameters are used to generate a reference trajectory for the robot's autonomous navigation within the trellis-type orchard rows.

[0078] First, a three-dimensional coordinate space containing x, y, and z axes is constructed, using the location of the robot equipped with LiDAR as the origin. Along the robot's direction of travel, the LiDAR scans the objects on both sides of the robot in real time, acquiring the corresponding raw point cloud data. The three-dimensional coordinate space is divided into numerous identical cubes, and a coordinate point is randomly selected from the raw point cloud data of each cube to form standard point cloud data. Based on the standard point cloud data, navigation baseline parameters are generated through multiple parallel line fitting operations. These parameters are then used to generate a reference trajectory for the robot's autonomous navigation within the trellis orchard rows. The robot receives and executes this reference trajectory to control its movement within the trellis orchard.

[0079] As the robot moves forward, its left side is designated as the first side, and its right side as the second side.

[0080] The navigation baseline parameters include: precise slope, precise left intercept, and precise right intercept.

[0081] The precise slope indicates the directional trend of the rows of fruit trees in an orchard, reflecting the direction of the rows. The precise slope is the slope of the centerline formed after fitting the rows of fruit trees.

[0082] The precise left intercept is the y-intercept of the straight line formed after fitting the first side of the fruit tree row. It determines the position of the first side of the fruit tree row relative to the origin of the three-dimensional coordinate space.

[0083] The precise right intercept is the y-intercept of the straight line formed after fitting the second side fruit tree row, which determines the position of the second side fruit tree row relative to the origin of the three-dimensional coordinate space.

[0084] By using LiDAR scanning to obtain the actual conditions within the orchard, the robot eliminates its reliance on RTK (Real-Time Kinematics). Voxelization downsampling effectively reduces data volume while preserving point cloud features, thus improving processing speed. Multiple parallel line fitting operations continuously filter more suitable coordinate points from the standard point cloud data, narrowing the data range. These filtered coordinate points are then used to calculate navigation baseline parameters, improving their accuracy and enabling more stable robot navigation in trellis-type orchard environments.

[0085] In some of these embodiments, such as Figure 2 As shown, the navigation reference parameter generation step further includes:

[0086] In the initial division step S201, each coordinate point in the standard point cloud data includes x, y, and z values. The coordinate points with y values ​​greater than zero in the standard point cloud data are selected and recorded as the first side standard point cloud set, and the coordinate points with y values ​​less than zero in the standard point cloud data are selected and recorded as the second side standard point cloud set.

[0087] In the preliminary fitting step S202, parallel lines are fitted based on the coordinate points in the first and second side standard point cloud sets, calculating the preliminary slope, preliminary left intercept, and preliminary right intercept. The first side parallel line is generated based on the preliminary slope and preliminary left intercept. The second side parallel line is generated based on the preliminary slope and preliminary right intercept.

[0088] Wherein, the preliminary slope is the slope of the first parallel line and the slope of the second parallel line, the preliminary left intercept is the y-intercept of the first parallel line, and the preliminary right intercept is the y-intercept of the second parallel line.

[0089] Specifically, the formula for calculating the initial slope through parallel line fitting is as follows:

[0090] .

[0091] in, This is the initial slope. This is the sum of the y-values ​​of all coordinate points in the first-side standard point cloud set. For accurate left intercept, The number of coordinate points in the first-side standard point cloud set. It is the sum of the x-values ​​of all coordinate points in the first-side standard point cloud set.

[0092] Specifically, the formula for calculating the initial left intercept of the first parallel line through parallel line fitting is as follows:

[0093] .

[0094] in, This is the initial left intercept, which is the y-intercept of the first parallel line. This is the sum of the y-values ​​of all coordinate points in the first-side standard point cloud set. This is the sum of the y-values ​​of all coordinate points in the second-side standard point cloud set. This is the sum of the squares of the x-values ​​of each coordinate point in the first and second standard point cloud sets. It is the sum of the products of the x and y values ​​of each coordinate point in the first and second standard point cloud sets. This is the sum of the x-values ​​of all coordinate points in the first-side standard point cloud set. This is the sum of the x-values ​​of all coordinate points in the second-side standard point cloud set. The number of coordinate points in the first-side standard point cloud set. This represents the number of coordinate points in the second-side standard point cloud set.

[0095] Specifically, the formula for calculating the initial right intercept of the second parallel line through parallel line fitting is as follows:

[0096] .

[0097] in, This is the initial right intercept, that is, the y-intercept of the second parallel line. This is the sum of the y-values ​​of all coordinate points in the second-side standard point cloud set. This is the initial slope. This is the sum of the x-values ​​of all coordinate points in the second-side standard point cloud set. This represents the number of coordinate points in the second-side standard point cloud set.

[0098] Based on the initial slope and initial left intercept, the first side parallel line is generated, and the first side parallel line is denoted as: .

[0099] Based on the initial slope and initial right intercept, a second parallel line is generated, denoted as: .

[0100] In the precise segmentation step S203, the distance from each coordinate point in the first-side standard point cloud set to the first-side parallel line is calculated. The distance from each coordinate point in the second-side standard point cloud set to the second-side parallel line is also calculated. A first distance threshold is set, and coordinate points in the first-side standard point cloud set whose distance values ​​are less than the first distance threshold are selected to generate the first-side precise point cloud set. Similarly, coordinate points in the second-side standard point cloud set whose distance values ​​are less than the first distance threshold are selected to generate the second-side precise point cloud set.

[0101] The first distance threshold can be set according to the actual effect, and the value range is [0.1, 0.5].

[0102] In the precise fitting step S204, based on the first side precise point cloud set and the second side precise point cloud set, the navigation baseline parameters are calculated through parallel line fitting, and the navigation baseline parameters are sent to the robot.

[0103] Phased fitting can gradually eliminate interference from outliers and improve the accuracy of the navigation baseline. Setting a distance threshold helps to filter out effective point cloud data and enhances the system's adaptability to the complex environment of orchards.

[0104] The navigation baseline parameters include precise slope, precise left intercept, and precise right intercept. Precise slope refers to the slope of the straight line formed by precisely fitting the first and second side precise point cloud sets. Precise left intercept is the y-intercept of the straight line formed by precisely fitting the first side precise point cloud set. Precise right intercept is the y-intercept of the straight line formed by precisely fitting the second side precise point cloud set.

[0105] In some embodiments, the accurate fitting step further includes:

[0106] The formula for calculating the precise slope using parallel line fitting is:

[0107] .

[0108] in, For precise slope, This is the sum of the y-values ​​of all coordinate points in the first side's precise point cloud set. For accurate left intercept, The number of coordinate points in the first side's precise point cloud set. It is the sum of the x-values ​​of all coordinate points in the first side of the precise point cloud set.

[0109] The formula for calculating the precise slope is derived based on the least squares method. It obtains the optimal slope estimate by minimizing the sum of squares of the perpendicular distances from a point to a line. Using a unilateral point cloud set to calculate the slope can reduce the impact of inconsistent or asymmetrical spacing between rows of fruit trees on navigation.

[0110] In some embodiments, the accurate fitting step further includes:

[0111] The formula for calculating the precise left intercept using parallel line fitting is as follows:

[0112] .

[0113] in, For accurate left intercept, This is the sum of the y-values ​​of all coordinate points in the first side's precise point cloud set. This is the sum of the y-values ​​of all coordinate points in the second-side precise point cloud set. This is the sum of the squares of the x-values ​​of each coordinate point in the first and second side precise point cloud sets. It is the sum of the products of the x and y values ​​of each coordinate point in the first and second side precise point cloud sets. This is the sum of the x-values ​​of all coordinate points in the first side's precise point cloud set. This is the sum of the x-values ​​of all coordinate points in the second side's precise point cloud set. The number of coordinate points in the first side's precise point cloud set. This represents the number of coordinate points in the second side's precise point cloud set.

[0114] By comprehensively considering the information from the point clouds on both sides, the weights of the data on both sides can be balanced, thereby improving the stability of the intercept calculation. The accurate calculation of the intercept plays a crucial role in determining the robot's center position between the two rows of fruit trees, helping to avoid collisions between the robot and the fruit trees.

[0115] In some embodiments, the accurate fitting step further includes:

[0116] The formula for calculating the precise right intercept using parallel line fitting is as follows:

[0117] .

[0118] in, To achieve accurate right intercept, This is the sum of the y-values ​​of all coordinate points in the second-side precise point cloud set. For precise slope, This is the sum of the x-values ​​of all coordinate points in the second-side precise point cloud set. This represents the number of coordinate points in the second side's precise point cloud set.

[0119] By using the precisely calculated slope to determine the accurate right intercept, the parallelism of the two parallel lines is ensured, meeting the actual needs of orchard row navigation. This method of calculating the intercept on each side can adapt to situations where fruit tree rows are not parallel or are curved in the orchard, enhancing the robustness of the navigation system.

[0120] Based on the navigation baseline parameters, two parallel lines can be constructed as navigation baselines. These two parallel lines represent the center lines of the rows of fruit trees on either side of the robot. The robot travels along the midpoint of these two parallel lines, that is, it moves along the middle of the rows of fruit trees. Based on these two parallel lines, the ideal center line of the robot can be constructed. .

[0121] The precise slope determines the robot's direction of travel, and the robot's heading angle is adjusted to correspond to this precise slope, ensuring that the robot remains parallel to the rows of fruit trees. The formula for calculating the heading angle is: .

[0122] The lateral position is adjusted by comparing the robot's current position with the ideal centerline. For example, the robot's current position is... The y-value corresponding to the ideal center line is The lateral offset is The robot makes lateral adjustments based on this offset, so that... Approaching 0.

[0123] During the robot's movement, the LiDAR continuously updates the point cloud data and calculates new navigation reference parameters in real time. Based on the latest parameters, the robot continuously adjusts its heading angle and lateral position to adapt to any curves or unevenness that may exist in the rows of fruit trees.

[0124] like Figure 5As shown, standard point cloud data is obtained through point cloud data processing step S102. Based on the positive or negative y value of each coordinate point in the standard point cloud data, the first side standard point cloud set and the second side standard point cloud set are initially divided. Coordinate points with y < 0 are divided into the second side standard point cloud set, and coordinate points with y > 0 are divided into the first side standard point cloud set.

[0125] Based on the initially divided first-side and second-side standard point cloud sets, preliminary slope, preliminary left intercept, and preliminary right intercept are obtained through parallel line fitting, generating first-side and second-side parallel lines. The distances from each coordinate point in the first-side standard point cloud set to the first-side parallel line and from each coordinate point in the second-side standard point cloud set to the second-side parallel line are calculated. Based on distance thresholds, a first-side precise point cloud set is selected from the first-side standard point cloud set, and a second-side precise point cloud set is selected from the second-side standard point cloud set. Finally, parallel line fitting is performed again based on the first-side precise point cloud set and the second-side standard point cloud set to obtain precise slope, precise left intercept, and precise right intercept, which serve as the final navigation line fitting reference parameters, and valid identifiers are set.

[0126] The accuracy of the left and right point cloud partitioning directly determines the accuracy of the navigation lines. Through multiple iterations and optimizations of the partitioning, more accurate left and right point cloud data were obtained. Then, the navigation baseline parameters, namely the accurate slope and accurate left and right intercepts, were obtained by using a parallel line fitting method, ensuring that the robot always travels in the middle of the tree row.

[0127] In some embodiments, the navigation method further includes:

[0128] The ground detection step involves performing ground detection on the standard point cloud data. If no ground is detected, invalid navigation parameters and an invalid identifier are output. If ground is detected, a valid identifier is output.

[0129] The system controls the robot's operation within the trellis-style orchard based on valid markers and navigation baseline parameters. Invalid markers indicate that the robot does not use invalid navigation parameters. Valid markers indicate that the robot uses the navigation baseline parameters.

[0130] Before point cloud data processing step S102, a ground detection step is performed. Since the LiDAR detects the point cloud data in the orchard in real time, to ensure the authenticity of the point cloud data, ground detection is performed on each frame of point cloud data. If the ground is not detected, invalid navigation parameters and invalid identifiers are output. If the ground is detected, the point cloud data of that frame is further processed and a valid identifier is output to the robot.

[0131] When an invalid flag is output, it indicates that the point cloud data acquired in the current frame cannot meet the navigation requirements, the point cloud data of this frame is unavailable, the invalid navigation parameters are unreliable, and the robot does not operate according to the invalid navigation parameters.

[0132] When a valid identifier is output, it indicates that ground detection was successful, signifying that the point cloud data acquired in the current frame meets the quality standards, the point cloud data of this frame is usable, and the navigation baseline parameters are reliable. The navigation baseline parameters obtained by fitting the point cloud data of this frame and their valid identifiers are both sent to the robot. The robot determines whether the navigation baseline parameters can be used based on the valid identifiers and operates in the orchard according to the navigation baseline parameters.

[0133] Ground detection serves as a crucial verification step for navigation effectiveness. In specific scenarios, such as when a robot is climbing a slope or when obstacles on the ground obstruct the LiDAR's line of sight, it can promptly detect and prevent erroneous navigation. By outputting clear valid or invalid identifiers, it provides a reliable basis for the robot's decision-making system, reducing the risk of misoperation.

[0134] In some embodiments, the navigation method further includes:

[0135] The ground detection step involves setting the detection range and a second distance threshold, comparing the x-values ​​in the standard point cloud data within the detection range to obtain the maximum x-value, and determining whether the maximum x-value is less than the second distance threshold. If so, the robot is controlled to stop or turn around.

[0136] The end-of-row detection function enables robots to automatically identify the end of orchard rows, allowing them to turn around or stop autonomously, reducing human intervention and improving the automation level of orchard operations. This detection method based on x-values ​​does not rely on additional markers or positioning equipment, and has good versatility and adaptability.

[0137] By analyzing the spatial distribution and quantity characteristics of point clouds, combined with the radar's field of view and detection range, the system accurately determines whether the robot has reached the end of the field. This method avoids the signal instability problem inherent in RTK solutions and is adaptable to complex orchard environments.

[0138] like Figure 6 As shown, after acquiring the raw point cloud data, coordinate points in the standard point cloud data are selected according to the detection range to obtain coordinate points within F meters in front of the lidar along the positive x-axis direction. The value range of F is [20, 30]. The x-values ​​of all coordinate points within the detection range are compared to obtain the maximum x-value. The coordinate point corresponding to the maximum x-value is the coordinate point farthest from the lidar in front of the lidar. The maximum x-value is used as the distance from the robot to the ground. If the distance from the robot to the ground is less than the second distance threshold, the value range of the second distance threshold is [0, 3], then it is considered that the robot has reached or is close to the ground, thereby triggering the robot to stop or turn around.

[0139] like Figure 4As shown, this application can also first acquire the original point cloud data, further determine whether the ground has been reached, and perform ground detection. If the ground has been reached, the calculation ends; if the ground has not been reached, ground detection is performed to determine whether the ground has been detected. If not, an invalid flag is output and the calculation ends; if so, a valid flag is output, and further point cloud filtering is performed to select suitable point cloud data. Further point cloud voxel downsampling is performed to simplify the point cloud data. Based on the location of the radar, the point cloud on the left side of the LiDAR is divided into a first-side standard point cloud set, and the point cloud on the right side of the LiDAR is divided into a second-side standard point cloud set. Two parallel line fitting operations are performed to obtain the navigation baseline parameters. Finally, the navigation baseline parameters and valid flags are output to the robot.

[0140] It should be noted that the steps shown in the above process or in the flowchart of the accompanying figures can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0141] This application also provides an orchard row navigation system, including an autonomous navigation robot equipped with LiDAR in a trellis orchard. Figure 3 As shown, the navigation system includes:

[0142] The point cloud data acquisition module 301 is configured to take the position of the robot with LiDAR as the origin and form a three-dimensional coordinate space including the x-axis, y-axis and z-axis. It scans the objects on both sides of the robot in real time and acquires the original point cloud data corresponding to the objects.

[0143] Point cloud data processing module 302, connected to point cloud data processing module 302, is configured to acquire three-dimensional coordinate space and raw point cloud data, divide the three-dimensional coordinate space into multiple cubes of equal size, and randomly select a coordinate point from the raw point cloud data in each cube to form standard point cloud data.

[0144] The navigation reference parameter generation module 303, connected to the point cloud data processing module 302, is configured to acquire standard point cloud data, and based on parallel line fitting, filter coordinate points from the standard point cloud data for calculating navigation reference line parameters. Based on parallel line fitting, navigation reference line parameters are generated according to the selected coordinate points. These navigation reference line parameters are used to generate a reference trajectory for the robot's autonomous navigation within the trellis-type orchard rows.

[0145] The modular design makes the system structure clearer, reduces the coupling between modules, and facilitates independent optimization and maintenance. This architecture can be easily integrated with different types of robot platforms and has good scalability.

[0146] In some embodiments, the navigation system further includes:

[0147] The ground detection module, connected to the point cloud data processing module 302 and the robot, is configured to perform ground detection on standard point cloud data. If no ground is detected, invalid navigation parameters and an invalid identifier are output to the robot. If ground is detected, a valid identifier is output to the robot.

[0148] The system controls the robot's operation within the trellis-style orchard based on valid markers and navigation baseline parameters. Invalid markers indicate that the robot does not use invalid navigation parameters. Valid markers indicate that the robot uses the navigation baseline parameters.

[0149] By adding a ground detection module to the original system, a closed-loop verification mechanism is formed, improving the safety and reliability of navigation. This module can reuse the output results of the point cloud data processing module 302, eliminating the need for additional hardware and reducing system costs.

[0150] In some embodiments, the navigation system further includes:

[0151] The ground detection module, which connects the point cloud data processing module 302 and the robot, is configured to set a detection range and a second distance threshold, compare the x-values ​​in the standard point cloud data within the detection range to obtain the maximum x-value, and determine whether the maximum x-value is less than the second distance threshold. If so, it controls the robot to stop or turn around.

[0152] The field edge detection module is deeply integrated with the navigation system, achieving full automation from in-row navigation to field edge turning. The configurable thresholds allow the system to adapt to orchards with different planting spacing and shapes, enhancing its flexibility.

[0153] It should be noted that the above modules can be functional modules or program modules, and can be implemented through software or hardware. For modules implemented through hardware, the above modules can reside in the same processor; or the above modules can be located in different processors in any combination.

[0154] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0155] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this patent application should be determined by the appended claims.

Claims

1. A method for orchard row navigation, used for an autonomous navigation robot equipped with LiDAR in a trellis orchard, characterized in that, The navigation method includes: The point cloud data acquisition step involves taking the location of the robot equipped with lidar as the origin and constructing a three-dimensional coordinate space including the x-axis, y-axis, and z-axis. The robot scans the objects on both sides in real time to acquire the original point cloud data corresponding to the objects. The point cloud data processing steps involve dividing the three-dimensional coordinate space into multiple cubes of equal size, and randomly selecting a coordinate point from the original point cloud data in each cube to form standard point cloud data. The navigation reference parameter generation step involves selecting coordinate points from the standard point cloud data for calculating navigation reference line parameters; and generating navigation reference line parameters based on parallel line fitting, according to the selected coordinate points. The navigation reference line parameters are used to generate a reference trajectory for the robot's autonomous navigation within the trellis-type orchard rows. The navigation reference parameter generation step further includes: The initial division steps are as follows: each coordinate point in the standard point cloud data includes x, y and z values. The coordinate points in the standard point cloud data with y values ​​greater than zero are selected as the first side standard point cloud set, and the coordinate points in the standard point cloud data with y values ​​less than zero are selected as the second side standard point cloud set. The preliminary fitting step involves performing parallel line fitting on the coordinate points in the first side standard point cloud set and the coordinate points in the second side standard point cloud set, calculating the preliminary slope, preliminary left intercept, and preliminary right intercept; generating a first-side parallel line based on the preliminary slope and the preliminary left intercept; and generating a second-side parallel line based on the preliminary slope and the preliminary right intercept. Wherein, the preliminary slope is the slope of the first side parallel line and the slope of the second side parallel line, the preliminary left intercept is the intercept of the first side parallel line on the y-axis, and the preliminary right intercept is the intercept of the second side parallel line on the y-axis; The precise division steps involve calculating the distance from each coordinate point in the first-side standard point cloud set to the first-side parallel line; calculating the distance from each coordinate point in the second-side standard point cloud set to the second-side parallel line; setting a first distance threshold; selecting coordinate points in the first-side standard point cloud set whose distance values ​​are less than the first distance threshold to generate a first-side precise point cloud set; and selecting coordinate points in the second-side standard point cloud set whose distance values ​​are less than the first distance threshold to generate a second-side precise point cloud set. The precise fitting step involves calculating the navigation baseline parameters based on the first side precise point cloud set and the second side precise point cloud set through parallel line fitting, and then sending the navigation baseline parameters to the robot. The navigation baseline parameters include precise slope, precise left intercept, and precise right intercept; the precise slope is the slope of the straight line formed by precise fitting of the first side precise point cloud set and the slope of the straight line formed by precise fitting of the second side precise point cloud set; the precise left intercept is the intercept of the straight line formed by precise fitting of the first side precise point cloud set on the y-axis; and the precise right intercept is the intercept of the straight line formed by precise fitting of the second side precise point cloud set on the y-axis.

2. The orchard row navigation method according to claim 1, characterized in that, The precise fitting step further includes: The formula for calculating the precise slope using parallel line fitting is as follows: ； in, For the precise slope, The sum of the y-values ​​of all coordinate points in the first side's precise point cloud set. For the precise left intercept, The number of coordinate points in the first side's precise point cloud set. It is the sum of the x-values ​​of all coordinate points in the first side's precise point cloud set.

3. The orchard row navigation method according to claim 1, characterized in that, The precise fitting step further includes: The formula for calculating the precise left intercept using parallel line fitting is as follows: ； in, For the precise left intercept, The sum of the y-values ​​of all coordinate points in the first side's precise point cloud set. This is the sum of the y-values ​​of all coordinate points in the second side's precise point cloud set. It is the sum of the squares of the x-values ​​of each coordinate point in the first side's precise point cloud set and the second side's precise point cloud set. It is the sum of the products of the x and y values ​​of each coordinate point in the first side's precise point cloud set and the second side's precise point cloud set. This is the sum of the x-values ​​of all coordinate points in the first side's precise point cloud set. This is the sum of the x-values ​​of all coordinate points in the second side's precise point cloud set. The number of coordinate points in the first side's precise point cloud set. This represents the number of coordinate points in the second side's precise point cloud set.

4. The orchard row navigation method according to claim 1, characterized in that, The precise fitting step further includes: The formula for calculating the precise right intercept using parallel line fitting is as follows: ； in, The precise right intercept, This is the sum of the y-values ​​of all coordinate points in the second side's precise point cloud set. For the precise slope, This is the sum of the x-values ​​of all coordinate points in the second side's precise point cloud set. This represents the number of coordinate points in the second side's precise point cloud set.

5. The orchard row navigation method according to any one of claims 1-4, characterized in that, Also includes: The ground detection step involves performing ground detection on the standard point cloud data. If no ground is detected, invalid navigation parameters and invalid identifiers are output. If ground is detected, a valid identifier is output; The robot is controlled to operate within the trellis orchard based on the valid identifier and the navigation baseline parameters. The invalid identifier indicates that the robot does not use the invalid navigation parameters, while the valid identifier indicates that the robot uses the navigation baseline parameters.

6. The orchard row navigation method according to any one of claims 1-4, characterized in that, Also includes: The ground detection step involves setting a detection range and a second distance threshold, comparing the x-values ​​in the standard point cloud data within the detection range to obtain the maximum x-value, and determining whether the maximum x-value is less than the second distance threshold. If so, the robot is controlled to stop or turn around.

7. A navigation system for orchard rows, used for an autonomous navigation robot equipped with lidar in a trellis orchard, characterized in that, The navigation system includes: The point cloud data acquisition module is configured to take the position of the robot with lidar as the origin, form a three-dimensional coordinate space including the x-axis, y-axis and z-axis, and scan the objects on both sides of the robot in real time to acquire the original point cloud data corresponding to the objects. A point cloud data processing module, connected to the point cloud data processing module, is configured to acquire the three-dimensional coordinate space and the original point cloud data, divide the three-dimensional coordinate space into multiple cubes of equal size, and randomly select a coordinate point from the original point cloud data in each cube to form standard point cloud data. The navigation reference parameter generation module, connected to the point cloud data processing module and the robot, is configured to acquire standard point cloud data, filter coordinate points from the standard point cloud data based on parallel line fitting for calculating navigation reference line parameters, and generate navigation reference line parameters based on the selected coordinate points. The navigation reference line parameters are used to generate a reference trajectory for the robot to autonomously navigate within the rows of the trellis-type orchard. The navigation reference parameter generation module is further configured as follows: Each coordinate point in the standard point cloud data includes an x ​​value, a y value, and a z value. The coordinate points in the standard point cloud data with a y value greater than zero are selected as the first side standard point cloud set, and the coordinate points in the standard point cloud data with a y value less than zero are selected as the second side standard point cloud set. Parallel lines are fitted to the coordinate points in the first side standard point cloud set and the coordinate points in the second side standard point cloud set to calculate the preliminary slope, preliminary left intercept, and preliminary right intercept; a first side parallel line is generated based on the preliminary slope and the preliminary left intercept; a second side parallel line is generated based on the preliminary slope and the preliminary right intercept. Wherein, the preliminary slope is the slope of the first side parallel line and the slope of the second side parallel line, the preliminary left intercept is the intercept of the first side parallel line on the y-axis, and the preliminary right intercept is the intercept of the second side parallel line on the y-axis; Calculate the distance from each coordinate point in the first side standard point cloud set to the first side parallel line; calculate the distance from each coordinate point in the second side standard point cloud set to the second side parallel line; set a first distance threshold, select coordinate points in the first side standard point cloud set whose distance values ​​are less than the first distance threshold, and generate a first side precise point cloud set; select coordinate points in the second side standard point cloud set whose distance values ​​are less than the first distance threshold, and generate a second side precise point cloud set. Based on the first side precise point cloud set and the second side precise point cloud set, the navigation baseline parameters are calculated by fitting parallel lines, and the navigation baseline parameters are sent to the robot. The navigation baseline parameters include precise slope, precise left intercept, and precise right intercept; the precise slope is the slope of the straight line formed by precise fitting of the first side precise point cloud set and the slope of the straight line formed by precise fitting of the second side precise point cloud set; the precise left intercept is the intercept of the straight line formed by precise fitting of the first side precise point cloud set on the y-axis; and the precise right intercept is the intercept of the straight line formed by precise fitting of the second side precise point cloud set on the y-axis.

8. The orchard row navigation system according to claim 7, characterized in that, Also includes: The ground detection module, connected to the point cloud data processing module and the robot, is configured to perform ground detection on the standard point cloud data. If no ground is detected, invalid navigation parameters and invalid identifiers are output to the robot; if ground is detected, valid identifiers are output to the robot. The robot is controlled to operate within the trellis orchard based on the valid identifier and the navigation baseline parameters. The invalid identifier indicates that the robot does not use the invalid navigation parameters, while the valid identifier indicates that the robot uses the navigation baseline parameters.

9. The orchard row navigation system according to claim 7, characterized in that, Also includes: The ground detection module, connected to the point cloud data processing module and the robot, is configured to set a detection range and a second distance threshold, compare the x-values ​​in the standard point cloud data within the detection range to obtain the maximum x-value, determine whether the maximum x-value is less than the second distance threshold, and if so, control the robot to stop or turn around.

Images