Autonomous navigation operation method for robot in orchard environment

Abstract

The invention discloses a robot autonomous navigation operation method in an orchard environment, and the method comprises the following steps: S1, generating an initial shortest path; s2, performing segmentation processing on the initial shortest path at a specified length; s3, carrying out bidirectional continuation along the normal direction to generate a strip-shaped polygonal region; s4, constructing a discretized scanning line cluster to solve an intersection point; s5, dynamically adjusting a passing area and re-planning a path; s6, carrying out global path smoothing processing; a path with continuous curvature can be generated, a safe distance is kept between the path and more corners, fruit trees or obstacles and the like, and the robot is safer and more reliable when working in an orchard.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of orchard robot navigation, and particularly relates to a method for autonomous navigation of a robot in an orchard environment. BACKGROUND

[0002] With the transformation and upgrading of orchard robots towards intelligence, autonomous navigation technology has become a core technology of intelligence. As an important part of the core technology, global path planning provides a strong guarantee for the autonomous operation of robots in orchards. The global path planning method in the prior art, such as the A-star algorithm, relies on the discrete expression of the grid map, and the path does not maintain a safe distance from the fruit trees, obstacles and the like, which is prone to collision. SUMMARY

[0003] This section aims to summarize some aspects of the embodiments of the present application and briefly introduce some preferred embodiments. Some simplifications or omissions may be made in this section and the abstract and title of the specification to avoid obscuring the purpose of this section, abstract and title, and such simplifications or omissions cannot be used to limit the scope of the present application.

[0004] In view of the above and / or existing problems in the prior art of global path planning of robots in a fruit tree environment, the present application is proposed.

[0005] Therefore, the purpose of the present application is to overcome the deficiencies in the prior art, and the present application provides a method for autonomous navigation of a robot in an orchard environment. The present application constructs a strip-shaped polygonal work area, and based on the minimum width of the area, introduces a passability verification to prevent collision, generates a passable path, and realizes that the planned path maintains sufficient safety distance from the fruit trees, obstacles and the like on both sides, without collision risk, solving the technical problem that the path in the prior art does not maintain a safe distance from the fruit trees, obstacles and the like, and is prone to collision.

[0006] The present application provides a method for autonomous navigation of a robot in an orchard environment, comprising the following steps,

[0007] S1, generating an initial shortest path;

[0008] S2, initial shortest path length segmentation processing;

[0009] S3, generating a strip-shaped polygonal area along the normal bidirectional extension;

[0010] S4, constructing a discrete scanning line cluster to solve the intersection point;

[0011] S5, dynamically adjusting the passable area and re-planning the path;

[0012] S6, global path smoothing processing.

[0013] As a preferred solution of the robot autonomous navigation operation method in the orchard environment of the present invention, wherein:

[0014] In step S1, based on the constructed map coordinate system, before path planning, the robot's initial position is taken as the starting point, the target point is specified, and all nodes of the map are hierarchically traversed from the initial point. The starting point is the 0th layer, its directly adjacent nodes are the 1st layer, the adjacent nodes of the adjacent nodes are the 2nd layer, and so on, until the target point is found for the first time. This path is the shortest path between the initial point and the target point. The generated shortest path consists of several coordinate points, path={(x0,y0), (x1,y1), ..., (x m ,y m )}, (x0,y0) is the initial point, (x m ,y m ) is the specified operation target point, and m is the number of coordinate points.

[0015] As a preferred solution of the robot autonomous navigation operation method in the orchard environment of the present invention, wherein:

[0016] Step S2 specifically includes:

[0017] S201, calculate the shortest path cumulative arc length s k , ,(x i ,y i ) is the coordinate of the i-th point in the path, (x i-1 ,y i-1 ) is the coordinate of the i-1th point, k≤m;

[0018] S202, determine the path segmentation threshold, set the standard segmentation threshold T, take the initial point (x0, y0) as the starting point of the first segment, and find the path segmentation threshold that meets s through step S201. k =T and take it as the first segment node, and use the previous segment node as the starting point to find the node that satisfies s through step S201. k =T's path point is used as the starting point of the second segment, and the above steps are repeated until all points in the path are traversed;

[0019] S203: When the length between the last segment node and the end point of the path is not greater than δT, where δ is a preset end margin coefficient, the last segment node is canceled and the remaining segment is merged into the previous segment.

[0020] S204, based on the path segmentation node, the initial shortest path is divided into several sub-segments, each sub-segment is expressed as path n ={(x0 ’ ,y0 ’ ), (x1’ ,y1 ’ ), ..., (x n ’ ,y n ’ )}, n is the number of coordinate points of the sub-segment.

[0021] As a preferred solution of the robot autonomous navigation operation method in the orchard environment of the present invention, step S3 is specifically as follows:

[0022] S301, calculate the unit left normal vector n of each path sub-segment L and the unit right normal vector n R , the normal vector is perpendicular to the path segment and points to the left and right sides;

[0023] S302: offset each path segment along the left and right normal directions by the width of the fruit tree row, d, to generate an outer offset line point set path. out and inner offset line point set path in , path out = path n *n L *d,path in = path n *n R *d;

[0024] S303: Connect the starting node, pathout, the ending segment node, and pathin of the path sub-segment in sequence according to the closed-loop path to construct a strip polygonal area covering both sides of the path, and mark obstacle information therein.

[0025] As a preferred solution of the robot autonomous navigation operation method in the orchard environment of the present invention, wherein:

[0026] Step S4 is specifically:

[0027] S401, solving the scan line slope k corresponding to each point on the path t , , t∈[1,n-1],(x t-1 ,y t-1 ) is the path path n The coordinates of the previous path point of point t, (x t+1 ,y t+1 ) is the path path n The coordinates of the next path point of point t, but when y i+1 -y i-1 =0, then k i Undefined, the scan line is perpendicular to the x-axis;

[0028] S402. When the scan line is not perpendicular to the x-axis, the scan line equation is , b f is the intercept corresponding to the scan line, and N is the number of scan lines. When the scan line is perpendicular to the x-axis, the scan line equation is x=m', where m' is the x-coordinate corresponding to the current scan line.

[0029] S403, solve the intersection of each scan line with the region boundary and obstacles, that is, the points where the boundary point set and the internal obstacle point set satisfy the scan line equation, and retain the two adjacent points with the largest distance, that is, the points that satisfy, , forming a new discretized boundary point set, where p is a natural number between 1 and n.

[0030] As a preferred solution of the robot autonomous navigation operation method in the orchard environment of the present invention, wherein:

[0031] Step S5 specifically includes:

[0032] S501, sequentially connect the path sub-segments starting node - Pout - ending node - Pin to generate a new path area;

[0033] S502: Based on the minimum width L of the new path area min , robot minimum turning radius R min , operating width M and collision prevention safety threshold σ, verify the feasibility of the expanded path area: when L min ≤min(R min +σ,M+σ), the passability verification fails, and this area is marked as occupied on the grid map and the global path replanning is triggered to generate an optimized path avoiding this area; when L min ≥min(R min +σ,M+σ), the feasibility verification is passed and the geometric center line of the new path area is extracted.

[0034] As a preferred solution of the robot autonomous navigation operation method in the orchard environment of the present invention, wherein:

[0035] In step S6, the tangent directions of the starting and ending nodes of the passable center lines of each path sub-segment extracted in step S5 are solved, and a transition curve is inserted at the connection of the center lines of the two path sub-segments to ensure that the tangent direction at the starting point of the transition curve is consistent with the tangent direction at the ending node of the center line of the previous path sub-segment, and at the same time ensure that the tangent direction at the ending point of the transition curve is consistent with the tangent direction at the starting node of the center line of the next path sub-segment, thereby smoothly connecting the center lines of each path sub-segment together, and finally the connected center lines are smoothed again as a whole to generate a smooth and continuous global navigation path.

[0036] As a preferred solution of the robot autonomous navigation operation method in the orchard environment of the present invention, before step S1, the following steps are also included:

[0037] S0. Environment map construction, the construction method includes:

[0038] S01. Use the laser radar to collect the three-dimensional point cloud data Point of the environment, and use the spherical neighborhood voxelization method to generate voxel point cloud. By setting the spherical radius Raduis, the point cloud space is divided into multiple voxels of equal volume. Then all points are assigned to the corresponding voxels according to the coordinates. By setting different sphere radii, point cloud sets of different scales B are generated. r , r represents different scales, corresponding to spheres of different radii;

[0039] S02: Point cloud collection B of different scales r , perform corrosion, expansion, opening and closing operations to obtain a new point cloud set B r ',

[0040]

[0041]

[0042]

[0043]

[0044] Where ⊝ is the corrosion operation, ⊕ is the expansion operation, ∘ is the opening operation, and • is the closing operation; B r (p) is a spherical neighborhood with a radius of r and a point as the center, and the set of points contained inside it. F is the operation threshold (only when B r (p) ≥ F, the point is retained); R space 3 is any point in a spherical three-dimensional space with a radius of r (not limited to the original point cloud);

[0045] S03: For point cloud sets B of different scales r , traverse each point in the set and calculate the corresponding curvature , the curvature is calculated as follows,

[0046] ;

[0047] ;

[0048] Where M matrix Represents the covariance matrix, Point c Indicates B r The center point, Pointa B represents r P represents a point other than the center point; λ1, λ2, λ3 are eigenvalues (λ1≥λ2≥λ3) of the matrix M matrix

[0049] For the point cloud curvature under different scales, weighted summation is performed to obtain the comprehensive curvature c multi

[0050]

[0051] In the formula, β r is the weight of the curvature on different scales;

[0052] S04: Based on the obtained point cloud curvature, the geometric characteristics (such as sharp edges, contour mutations, etc.) of the curvature representation are used to screen feature key points, and the corresponding relationship is constructed through the curvature-related descriptor, so as to realize the identification of the ground object target and construct a three-dimensional environment map;

[0053] S05: For the three-dimensional environment map constructed in step S04, the point cloud in the specified height range is projected onto a two-dimensional plane parallel to the ground by projection, and a two-dimensional map is generated for path planning. BRIEF DESCRIPTION OF DRAWINGS

[0054] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings needed in the embodiment description will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present application, and for those skilled in the art, other drawings can also be obtained without creative labor. Among them:

[0055] Figure 1 is a principle diagram of the present application.

[0056] Figure 2 is a schematic diagram of path segmentation and band-shaped polygon region generation in the present application.

[0057] Figure 3 is a schematic diagram of the final path generation using the present application.

[0058] Figure 4 is a comparison diagram of three-dimensional and two-dimensional environment maps constructed by the present application and LIO-SAM algorithm in a real orchard environment.

[0059] Figure 5 is a schematic diagram of obtaining a two-dimensional map of a real orchard environment and constructing a work path with different start and end points in it by the present application and A-star algorithm.

[0060] Figure 6 is a safety comparison diagram of the left side of the generated path.​​

[0061] Figure 7 To generate a security comparison chart on the right side of the path. DETAILED DESCRIPTION

[0062] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the specific implementation methods of the present invention are described in detail below in conjunction with the embodiments of the specification.

[0063] In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention may also be implemented in other ways different from those described herein. Those skilled in the art may make similar generalizations without violating the connotation of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below.

[0064] Secondly, the term "one embodiment" or "embodiment" herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The phrase "in one embodiment" appearing in various places throughout this specification does not necessarily refer to the same embodiment, nor does it refer to a separate or selective embodiment that is mutually exclusive of other embodiments.

[0065] Example 1: Reference Figures 1 to 3 , which is the first embodiment of the present invention, provides a method for autonomous navigation operation of a robot in an orchard environment, which can be implemented.

[0066] A method for autonomous navigation of a robot in an orchard environment comprises the following steps:

[0067] S0. Environment map construction, the construction method includes:

[0068] S01. Use the laser radar to collect the three-dimensional point cloud data Point of the environment, and use the spherical neighborhood voxelization method to generate voxel point cloud. By setting the spherical radius Raduis, the point cloud space is divided into multiple voxels of equal volume. Then all points are assigned to the corresponding voxels according to the coordinates. By setting different sphere radii, point cloud sets of different scales B are generated. r , r represents different scales, corresponding to spheres of different radii;

[0069] S02: Point cloud collection B of different scales r , perform corrosion, expansion, opening and closing operations to obtain a new point cloud set B r ',

[0070]

[0071]

[0072]

[0073]

[0074] Where ⊝ is the corrosion operation, ⊕ is the expansion operation, ∘ is the opening operation, and • is the closing operation; B r (p) is a spherical neighborhood with a radius of r and a point as the center, and the set of points contained inside it. F is the operation threshold (only when B r (p) ≥ F, the point is retained); R space 3 is any point in a spherical three-dimensional space with a radius of r (not limited to the original point cloud);

[0075] S03: For point cloud sets B of different scales r , traverse each point in the set and calculate the corresponding curvature , the curvature is calculated as follows,

[0076] ;

[0077] ;

[0078] Where M matrix Represents the covariance matrix, Point c Indicates B r The center point, Point a Indicates B r Other points except the center point; λ1, λ2, λ3 are the matrix M matrix eigenvalues ​​of (λ1≥λ2≥λ3);

[0079] For the point cloud curvature at different scales, perform weighted summation to obtain the comprehensive curvature c multi :

[0080]

[0081] Where, β r is the weight of curvature at different scales;

[0082] S04: Based on the obtained point cloud curvature, the geometric characteristics represented by curvature (such as sharp edges and sudden contour changes) are used to screen key feature points, and corresponding relationships are established through curvature-related descriptors to achieve ground object recognition and build a 3D environment map;

[0083] S05: For the 3D environment map constructed in step S04, project the point cloud within the specified height range onto a 2D plane parallel to the ground to generate a 2D map for path planning;

[0084] S1. Based on the constructed map coordinate system, before path planning, the robot's initial position is used as the starting point, the target point is specified, and the shortest path between the two points is generated (starting from the initial point, hierarchically traversing all nodes of the map, the starting point is the 0th layer, its directly adjacent nodes are the 1st layer, the adjacent nodes of the adjacent nodes are the 2nd layer, and so on, until the target point is found for the first time. This path is the shortest path between the initial point and the target point). The generated shortest path consists of several coordinate points, path={(x0,y0), (x1,y1), ..., (x m ,y m )}, (x0,y0) is the initial point, (x m ,y m ) is the designated operation target point, and m is the number of coordinate points;

[0085] S2. Initial shortest path specified length segment processing, specifically:

[0086] S201, calculate the shortest path cumulative arc length s k , ,(x i ,y i ) is the coordinate of the i-th point in the path, (x i-1 ,y i-1 ) is the coordinate of the i-1th point, which is the coordinate of the previous path point of the i-th point, k≤m;

[0087] S202, determine the path segmentation threshold, set the standard segmentation threshold T, take the initial point (x0, y0) as the starting point of the first segment, and find the path segmentation threshold that meets s through step S201. k =T and take it as the first segment node, and use the previous segment node as the starting point to find the node that satisfies s through step S201. k =T's path point is used as the starting point of the second segment, and the above steps are repeated until all points in the path are traversed;

[0088] S203: When the length between the last segment node and the end point of the path is not greater than δT, where δ is a preset end margin coefficient, the last segment node is canceled and the remaining segment is merged into the previous segment.

[0089] S204, based on the path segmentation node, the initial shortest path is divided into several sub-segments, each sub-segment is expressed as path n ={(x0 ’ ,y0 ’ ), (x1 ’ ,y1 ’ ), ..., (x n ’ ,y n ’)}, n is the number of coordinate points of the sub-segment;

[0090] S3. Generate a strip-shaped polygonal area by bidirectional extension along the normal direction, specifically:

[0091] S301, calculate the unit left normal vector n of each path sub-segment L and the unit right normal vector n R , the normal vector is perpendicular to the path segment and points to the left and right sides;

[0092] S302: offset each path segment along the left and right normal directions by the width of the fruit tree row, d, to generate an outer offset line point set path. out and inner offset line point set path in , path out = path n *n L *d,path in = path n *n R *d;

[0093] S303, connect the starting node of the path sub-segment, path out , end segment node (the start and end segment nodes here refer to: for each path segment, its start node and end segment node, that is, the first and last two segment nodes of this path segment) and path in , construct a strip polygon area covering both sides of the path and mark the obstacle information in it;

[0094] S4. Construct a discretized scan line cluster to solve the intersection point, specifically:

[0095] S401, solving the scan line slope k corresponding to each point on the path t , , t∈[1,n-1],(x t-1 ,y t-1 ) is the path path n The coordinates of the previous path point of point t, (x t+1 ,y t+1 ) is the path path n The coordinates of the next path point of point t, but when y i+1 -y i-1 =0, then k i Undefined, the scan line is perpendicular to the x-axis;

[0096] S402. When the scan line is not perpendicular to the x-axis, the scan line equation is , b fN is the number of scan lines, and m' is the x-coordinate corresponding to the current scan line when the scan line is perpendicular to the x-axis;

[0097] S403, solve the intersection points of each scan line and the region boundary and obstacles, that is, the point set of boundary points and the point set of internal obstacle points satisfy the scan line equation, keep the two points with the largest distance between the two adjacent points, P out and P in , taking the direction in which the robot travels along the shortest path as the front, P out , the point set composed of the left side of the two points with the largest distance obtained by the scan line in the direction along the path, P in , the point set composed of the right side of the two points with the largest distance obtained by the scan line in the direction along the path, the above left and right directions are perpendicular to the driving direction of the robot, that is, satisfy, , constitute a new discrete boundary point set, p is a natural number between 1 and n;

[0098] S5, dynamic adjustment of the passable area and path re-planning, specifically:

[0099] S501, sequentially connect the path sub-section start node—P out —end section node—P in , generate a new path area;

[0100] S502, based on the minimum width L min of the new path area, the minimum turning radius R min of the robot, the working width M and the collision prevention safety threshold σ, verify the passability of the expanded path area: when L min ≥min(R min +σ,M+σ), the passability verification is passed, and the geometric center line of the new path area is extracted; when L min ≤min(R min +σ,M+σ), the passability verification is not passed, mark this area as occupied on the grid map and trigger global path re-planning, that is, based on the modified map, generate an optimized path that avoids this area through step S1, repeat steps S2-S5 until the path passability verification is passed;

[0101] S6. Solve the tangent directions of the starting and ending nodes of the passable centerline of each path sub-segment extracted in step S5, insert a transition curve at the connection of the centerlines of the two path sub-segments, ensure that the tangent direction at the starting point of the transition curve is consistent with the tangent direction at the ending node of the centerline of the previous path sub-segment, and at the same time ensure that the tangent direction at the ending point of the transition curve is consistent with the tangent direction at the starting node of the centerline of the next path sub-segment, thereby smoothly connecting the centerlines of each path sub-segment together, and then smoothing the connected centerlines again as a whole, and finally generating a smooth and continuous global navigation path.

[0102] In the present invention, the collected three-dimensional point cloud data of the environment is divided into point cloud sets of different scales and corrosion, expansion, opening and closing operations are performed. The curvature of the point clouds at different scales is solved and weighted summation is performed to achieve accurate recognition of the target features of the ground objects, and construct a high-quality environmental map with clear inter-row features, no noise interference, and no ground point cloud residue.

[0103] like Figure 2 Create a non-completely symmetrical two-dimensional top-view map model with obstacles to simulate the real orchard operation environment. In this map, plan a shortest trajectory path from the starting point to the target point. The path consists of a series of coordinate points. The final global navigation path generated is as follows: Figure 3 Shown as the red line segment.

[0104] The present invention generates a dynamic pass area by bidirectional extension along the normal direction, combined with the calculation of the intersection of discrete scan line clusters, breaking through the limitations of traditional raster or grid processing. This method can adapt to the complexity of the terrain and significantly reduce the amount of calculation; the strip area generated by normal extension is used as a dynamic constraint boundary, which reduces the invalid search space and improves the convergence speed of the algorithm compared to traditional dynamic evaluation strategies (such as Gaussian mutation). At the same time, by constructing a strip polygon operation area and introducing a collision-preventing passability verification based on the minimum width of the area, a passable path is generated, ensuring that the planned path maintains a sufficient safety distance from fruit trees, obstacles, etc. on both sides, without the risk of collision; by smoothing the generated path, the path curvature is continuous and smooth, without obvious corners, solving the technical problems of insufficient path safety and motion continuity defects in the existing technology.

[0105] Example 2: Reference Figures 4 to 7 This embodiment provides a robot autonomous navigation operation method in an orchard environment. The difference from Example 1 is that it verifies the technical effect of this application through scientific experimental technical means.

[0106] In a real orchard environment, the environment is scanned to obtain point cloud data, and the three-dimensional and two-dimensional maps of the orchard environment are constructed using the method of the present invention and the LIO-SAM algorithm in the prior art, respectively. The results are as follows: Figure 4With the sample as the horizontal axis, the actual distance on both sides of the path planned by the present invention and A-star and the safety distance (safety distance is min (R min +σ,M+σ)) is the vertical coordinate, generating a safety comparison chart on the left side of the path ( Figure 6 ) and the safety comparison chart on the right side of the path ( Figure 7 ).

[0107] This example uses the same process as Example 1 to construct three-dimensional and two-dimensional environmental maps. These maps are compared with those constructed using the LIO-SAM algorithm, further verifying that the environmental maps constructed by the present invention have distinct inter-row features, are free of interfering noise points, and lack residual ground point clouds, resulting in high-quality mapping. The present invention generates a global operating path by planning a shortest trajectory from a starting point to a target point within the constructed two-dimensional map. This is then compared with the global operating path planned by the A-star algorithm, further verifying that the global operating path planned by the present invention maintains a relatively safe distance from obstacles on both sides, eliminating collision risks.

[0108] according to Figure 4 The point cloud of the environmental map constructed by the LIO-SAM algorithm is too dense, and the features between rows are not obvious. The two-dimensional map generated by this algorithm has obvious residual ground point cloud interference noise points. However, the map point cloud density constructed by the method of the present invention is small, the features between fruit tree rows are obvious, the two-dimensional map has no residual ground point cloud and interference noise points, the mapping quality is high, and it is suitable for subsequent path planning.

[0109] The paths planned by the two methods were verified through experiments. The path planned by the present invention has good smoothness, and the robot does not have obvious pauses or turns when driving along the trajectory; however, the path planned by the A-star algorithm has problems such as discontinuous curvature and too many corners, and the robot has obvious pauses and turns when driving.

[0110] according to Figure 6 and Figure 7 The preset safety distance on both sides of the robot is half the robot width plus a safety margin to prevent collisions. The difference between the distance on both sides of the path planned by the present invention and the safety distance is always positive, with a minimum value of 0.03m. The robot has no collision risk. However, the difference between the distance on both sides of the path planned by the A* algorithm and the safety distance has many negative values, with a minimum value of -0.29m, which poses a collision risk.

[0111] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention may be modified or replaced by equivalents without departing from the spirit and scope of the technical solutions of the present invention, which should all be included in the scope of the claims of the present invention.

Claims

1. A method for autonomous robot navigation in an orchard environment, characterized by: The following steps are included: S1, generate the initial shortest path; S2, initial shortest path specified length segment processing; S3, bidirectional extension along the normal direction to generate a strip polygonal area; S4, constructing a discretized scan line cluster to solve the intersection point; S5. Dynamically adjust the traffic area and replan the route; S6. Global path smoothing.

2. The method for autonomous robot navigation in an orchard environment as claimed in claim 1, wherein: In step S1, based on the constructed map coordinate system, before path planning, the robot's initial position is taken as the starting point, the target point is specified, and all nodes of the map are hierarchically traversed from the initial point. The starting point is the 0th layer, its directly adjacent nodes are the 1st layer, the adjacent nodes of the adjacent nodes are the 2nd layer, and so on, until the target point is found for the first time. This path is the shortest path between the initial point and the target point. The generated shortest path consists of several coordinate points, path={(x0,y0), (x1,y1), ...,(x m ,y m )}, (x0,y0) is the initial point, (x m ,y m ) is the specified operation target point, and m is the number of coordinate points.

3. The method for autonomous robot navigation in an orchard environment as claimed in claim 2, wherein: Step S2 specifically includes: S201, calculate the shortest path cumulative arc length s k , , k≤m,(x i ,y i ) is the coordinate of the i-th point in the path, (x i-1 ,y i-1 ) is the coordinate of point i-1; S202, determine the path segmentation threshold, set the standard segmentation threshold T, take the initial point (x0, y0) as the starting point of the first segment, and find the path segmentation threshold that meets s through step S201. k =T and take it as the first segment node, and use the previous segment node as the starting point to find the node that satisfies s through step S201. k =T's path point is used as the starting point of the second segment, and the above steps are repeated until all points in the path are traversed; S203: When the length between the last segment node and the end point of the path is not greater than δT, where δ is a preset end margin coefficient, the last segment node is canceled and the remaining segment is merged into the previous segment. S204, based on the path segmentation node, the initial shortest path is divided into several sub-segments, each sub-segment is expressed as path n ={(x0 ’ ,y0 ’ ), (x1 ’ ,y1 ’ ), ..., (x n ’ ,y n ’ )}, n is the number of coordinate points of the sub-segment.

4. The method for autonomous robot navigation in an orchard environment according to claim 3 is characterized in that: Step S3 specifically includes: S301, calculate the unit left normal vector n of each path sub-segment L and the unit right normal vector n R , the normal vector is perpendicular to the path segment and points to the left and right sides; S302: offset each path segment along the left and right normal directions by the width of the fruit tree row, d, to generate an outer offset line point set path. out and inner offset line point set path in , path out = path n *n L *d,path in = path n *n R *d; S303: Connect the starting node, pathout, the ending segment node, and pathin of the path sub-segment in sequence according to the closed-loop path to construct a strip polygonal area covering both sides of the path, and mark obstacle information therein.

5. The method for autonomous robot navigation in an orchard environment according to claim 4, wherein: Step S4 is specifically: S401, solving the scan line slope k corresponding to each point on the path t , ,,t∈[1,n-1],(x t-1 ,y t-1 ) is the path path n The coordinates of the previous path point of point t, (x t+1 ,y t+1 ) is the path path n The coordinates of the next path point of point t, but when y t+1 -y t-1 =0, then k t Undefined, the scan line is perpendicular to the x-axis; S402. When the scan line is not perpendicular to the x-axis, the scan line equation is , b f is the intercept corresponding to the scan line, and N is the number of scan lines. When the scan line is perpendicular to the x-axis, the scan line equation is x=m', where m' is the x-coordinate corresponding to the current scan line. S403, solve the intersection of each scan line with the region boundary and obstacles, that is, the points where the boundary point set and the internal obstacle point set satisfy the scan line equation, and retain the two adjacent points with the largest distance, that is, the points that satisfy, , forming a new discretized boundary point set, where p is a natural number between 1 and n.

6. The method for autonomous robot navigation in an orchard environment as claimed in claim 4, characterized in that: Step S5 specifically includes: S501, sequentially connect the path sub-segments starting node - Pout - ending node - Pin to generate a new path area; S502: Based on the minimum width L of the new path area min , robot minimum turning radius R min , operating width M and collision prevention safety threshold σ, verify the feasibility of the expanded path area: when L min ≤min(R min +σ,M+σ), the passability verification fails, and this area is marked as occupied on the grid map and the global path replanning is triggered to generate an optimized path avoiding this area; when L min ≥min(R min +σ,M+σ), the feasibility verification is passed and the geometric center line of the new path area is extracted.

7. The method for autonomous robot navigation in an orchard environment according to claim 4, characterized in that: In step S6, the tangent directions of the starting and ending nodes of the passable center lines of each path sub-segment extracted in step S5 are solved, and a transition curve is inserted at the connection of the center lines of the two path sub-segments to ensure that the tangent direction at the starting point of the transition curve is consistent with the tangent direction at the ending node of the center line of the previous path sub-segment, and at the same time ensure that the tangent direction at the ending point of the transition curve is consistent with the tangent direction at the starting node of the center line of the next path sub-segment, thereby smoothly connecting the center lines of each path sub-segment together, and finally the connected center lines are smoothed again as a whole to generate a smooth and continuous global navigation path.

8. The method for autonomous robot navigation in an orchard environment according to claim 2, characterized in that: Before step S1, The following steps are included: S0. Environment map construction, the construction method includes: S01. Use the laser radar to collect the three-dimensional point cloud data Point of the environment, and use the spherical neighborhood voxelization method to generate voxel point cloud. By setting the spherical radius Raduis, the point cloud space is divided into multiple voxels of equal volume. Then all points are assigned to the corresponding voxels according to the coordinates. By setting different sphere radii, point cloud sets of different scales B are generated. r , r represents different scales, corresponding to spheres of different radii; S02: Point cloud collection B of different scales r , perform corrosion, expansion, opening and closing operations to obtain a new point cloud set B r ', ； ； ； ； In the formula, ⊝ is the corrosion operation, ⊕ is the expansion operation, ∘ is the opening operation, and • is the closing operation; B r (p) is a spherical neighborhood with a radius of r and a point as the center, and the set of points contained inside it. F is the operation threshold (only when B r (p)≥F, the point is retained); R space 3 is any point in a spherical three-dimensional space with a radius of r (not limited to the original point cloud); S03: For point cloud sets B of different scales r , traverse each point in the set and calculate the corresponding curvature , the curvature is calculated as follows, ； ； Where M matrix Represents the covariance matrix, Point c Indicates B r The center point, Point a Indicates B r Other points except the center point; λ1, λ2, λ3 are the matrix M matrix eigenvalues ​​of (λ1≥λ2≥λ3); For the point cloud curvature at different scales, perform weighted summation to obtain the comprehensive curvature c multi : ； Where, β r is the weight of curvature at different scales; S04: Based on the obtained point cloud curvature, the geometric characteristics represented by curvature (such as sharp edges and sudden contour changes) are used to screen key feature points, and corresponding relationships are established through curvature-related descriptors to achieve ground object recognition and build a 3D environment map; S05: For the three-dimensional environment map constructed in step S04, project the point cloud within the specified height range onto a two-dimensional plane parallel to the ground to generate a two-dimensional map for path planning.

Images