Complex orchard scene-oriented hybrid sampling global path planning method

Abstract

The invention relates to the technical field of mobile robot autonomous navigation path planning, and discloses a complex orchard scene-oriented hybrid sampling global path planning method, which comprises the steps of adopting a Gaussian hybrid sampling strategy, switching between global uniform sampling and Gaussian offset sampling through a probability threshold, and considering global exploration and key area guidance; a dynamic node expansion strategy is adopted, the expansion step length is adaptively adjusted according to the distance between the expansion step length and a target point, and the expansion direction is optimized by fusing vectors facing a sampling point and the target point and introducing an obstacle influence factor; a layered collision detection strategy is adopted, and step-by-step filtering is carried out through rough detection based on a quadtree, secondary screening based on an axis alignment bounding box and accurate geometric judgment in sequence so as to improve the efficiency; and a dynamic iteration termination mechanism is adopted, and the minimum number of iterations and the path quality convergence state are combined as composite termination conditions. The method can quickly generate a smooth and safe path, and is especially suitable for structured scenes such as orchards.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of autonomous navigation path planning of mobile robots, in particular to a hybrid sampling global path planning method for complex orchard scenes. BACKGROUND

[0002] With the rapid development of smart agriculture and precision agriculture, autonomous navigation and operation of agricultural machinery in structured scenes such as orchards and greenhouses have become a key technology to improve the efficiency of agricultural production. Path planning, as the core of the autonomous navigation system, directly determines the efficiency, safety and intelligence level of agricultural operation.

[0003] In the existing path planning technology, the rapid expansion random tree (RRT) algorithm based on random sampling has made great progress in the field of robot motion planning in the past decade. However, the traditional RRT algorithm has inherent limitations when applied to orchard scenes with obvious channel characteristics. The global uniform sampling strategy used by the algorithm lacks directionality, resulting in a large number of sampling points scattered in the obstacle regions such as plants and tree crowns or regions that are not beneficial to path optimization. This makes the exploration process blind and the convergence speed slow.

[0004] At the same time, the traditional algorithm is also rigid in node expansion mechanism. The fixed expansion step length is difficult to adapt to complex environments. When the node is far from the target, a small step length will slow down the exploration process. When the node is close to the obstacle or target point, a large step length may cause collision or miss the best path point. This lack of adaptability in the expansion method makes the initial path generated contain a large number of redundant nodes and unnecessary inflection points, and the smoothness and quality of the path are not ideal, making it difficult to be directly applied to actual agricultural machinery control.

[0005] In addition, in a dense orchard environment, frequent collision detection can become a computational bottleneck for the entire planning process. If an accurate geometric judgment is made by traversing all obstacles in the environment for each potential node expansion, a large amount of computational resources will be consumed, seriously affecting the real-time performance of path planning. How to efficiently and accurately complete collision detection is a major challenge faced by existing technologies. SUMMARY

[0006] To overcome the shortcomings of the prior art, the present application provides a hybrid sampling global path planning method for complex orchard scenes, which solves the problems of slow convergence speed and poor path quality in the prior art.

[0007] To achieve the above purpose, the present application is implemented by the following technical scheme: a hybrid sampling global path planning method for complex orchard scenes, comprising the following steps: S1. Random sampling points are generated using a Gaussian mixture sampling strategy.

[0008] This strategy combines global uniform distribution sampling with local Gaussian distribution sampling. Specifically, in each sampling iteration, random numbers are generated. and with a preset probability threshold Compare them.

[0009] like Then in the entire feasible configuration space Perform uniform random sampling within the sample area to generate sampling points. : ; like Then, Gaussian bias sampling is performed, with the Gaussian distribution placed in a preset key area (if near the center of the circular passageway), and the mean value is taken from the center of this area. , It can be a set of means containing the center points of multiple key regions, with a predefined covariance matrix. Generate sampling points : ; In a two-dimensional space, the covariance matrix can be expressed as: ; In the formula, the covariance matrix It can be a complete matrix containing off-diagonal elements to represent channels in any direction, not just those parallel to the coordinate axes; and The sampling points are respectively at shaft and Standard deviation on the axis.

[0010] S2. New nodes are generated using a node expansion strategy that optimizes dynamic step size and direction.

[0011] For sampling points Find the node with the closest Euclidean distance in the random tree. and from To expand the strategy, the first step is adaptive step size adjustment, expanding the step size. According to the node With global target point The distance is calculated dynamically: ; in, and These are the minimum and maximum step sizes, respectively. is a proportionality coefficient, the strategy also introduces a direction optimization guide, which first determines a direction unit vector towards the randomly sampled point and a direction unit vector towards the global goal point , then, according to the distance to the nearest obstacle and a preset safety distance threshold an obstacle influence factor is calculated: ; the final extended direction unit vector is determined by the following formula: ; ; the position of the new node is: ; At the geometric level, the formula defines an idealized linear extension, however, when applying the present invention to actual agricultural machinery with nonholonomic constraints such as minimum turning radius, the implementation of this step needs to further combine the kinematic model of the vehicle.

[0012] In embodiments, the new node calculated by the above formula is not directly added as the final node to the random tree, but rather serves as a local goal guide point, and the actual node extension process is as follows: The system takes the configuration (including position and heading angle) of the node as the starting point, calls a steering function (Steering Function) which, based on the kinematic model of the vehicle (for example, using the Reed-Shepp path generator), calculates an actual drivable path segment that starts from , meets all kinematic constraints such as minimum turning radius, and has its endpoint as close as possible to .

[0013] Finally, the endpoint of this drivable path segment is added to the random tree as the real, physically reachable new node, in this way, the method of the present invention ensures that each tiny segment in the generated path strictly follows the kinematic characteristics of the agricultural machinery, ensuring the feasibility and practicality of the final planning result while maintaining efficient exploration direction.

[0014] This step also includes: Multiple candidate nodes with different directions are generated each time, the path cost of each candidate node is calculated, and the candidate node with the smallest path cost is selected as the new node to be connected to the tree.

[0015] S3, the collision detection strategy of quadtree combined with AABB is used for collision detection.

[0016] The strategy firstly constructs a quadtree spatial index for static obstacles in the environment in the initialization phase. When detecting the path segment connecting the parent node and the new node , firstly, the quadtree index is used to query the leaf node regions passed by the path segment, and then only for the obstacles located in these leaf node regions and whose axis-aligned bounding boxes (AABB) intersect with the AABB of the path segment, the fine geometric collision judgment is performed.

[0017] S4, the dynamic iteration termination mechanism driven by path quality evaluation is used to determine whether to terminate.

[0018] The mechanism sets a minimum iteration number and a path quality improvement threshold , and the termination of the algorithm needs to meet two conditions: The current iteration number is not less than .

[0019] When a feasible path is found, the quality of the current optimal path is evaluated. The path quality is calculated by the path length , smoothness , and distance from obstacles , etc. When the relative improvement rate of the optimal path quality in the last iterations is less than the threshold , it is determined that the path quality converges. When the above two conditions are met, the algorithm terminates and outputs the current optimal path.

[0020] Smoothness Smoothness is used to measure the degree of curvature of the path.

[0021] An ideal path should be as straight as possible to avoid sharp or frequent turns, so this indicator is usually quantified by accumulating the turning angles or curvatures of each waypoint on the path. A lower smoothness cost value represents a smoother path.

[0022] Suppose the path is composed of a series of ordered path points , the smoothness cost can be defined as the sum of all turning angles on the path. A commonly used calculation method is: ; Where: represents the th waypoint on the path; is the turning angle of the th waypoint.point to ; From point point to ; Represents the dot product of vectors; The vector represents the Euclidean norm (i.e., length); the fractional part calculates the length at waypoints. At this point, the cosine value of the angle between the vectors of the two path segments. When three points When the cosine is on a straight line, the value of the cosine is 1, then the cost of the term is... The larger the turning angle, the smaller the cosine value, and the greater the cost of this term, thus penalizing sharp turns.

[0023] Interval with obstacles This metric measures the safety of a path, which is the distance between the path and obstacles in the environment. A high-quality path should be within a walkable area and maintain a sufficiently large safety distance from all obstacles. This cost item penalizes paths that are too close to obstacles.

[0024] The cost of the distance between a point on the path and the nearest obstacle is usually defined as the reciprocal of the distance between that point and the nearest obstacle.

[0025] ; in, It is a path The minimum distance between all points and the nearest obstacle is calculated as follows: ; in: It is a path Any point on the path (in actual calculations, the path segment is usually discretized and sampled to approximate this). Represents the set of all obstacles in the environment. It is a set One of the obstacles; It is a point and obstacles The shortest Euclidean distance between them; Therefore, it represents the most dangerous distance between the entire path and all obstacles, i.e., the minimum safe distance; Inversely proportional to this minimum safe distance, the closer the path is to the obstacle ( The smaller the size, the higher its cost; It is a very small positive number (e.g.) ), used to prevent the path from contacting obstacles ( When the denominator is zero, the result may be in the case of a fraction.

[0026] The application provides a hybrid sampling global path planning method for a complex orchard scene. 1、The application adopts a Gaussian mixture sampling strategy, and combines a node expansion strategy of dynamic step length and direction optimization, the former improves the probability of generating effective sampling points in narrow channel areas such as orchard rows, and the latter guides the growth of the random tree to actively avoid obstacles and approach the target, the synergistic effect of the two significantly speeds up the discovery speed of the initial feasible path and the convergence efficiency of the subsequent path.

[0027] 2、The application adopts a collision detection strategy of quadtree combined with axis-aligned bounding box (AABB), through constructing a spatial index and performing hierarchical filtering, a large number of irrelevant obstacles can be excluded in advance before accurate collision judgment, so that the calculation overhead of collision detection is reduced from linear correlation with the total number of obstacles to logarithmic correlation, and the overall running efficiency of the algorithm in the obstacle dense environment is effectively improved.

[0028] 3、The application introduces a dynamic iteration termination mechanism driven by path quality evaluation, so that the algorithm can adaptively determine the termination time according to the actual convergence of the path quality, instead of using a fixed number of iterations. The mechanism ensures that the algorithm can balance between calculation efficiency and path solution quality in environments of different complexities, avoids unnecessary consumption of computing resources, and at the same time guarantees the quality of the final output path. BRIEF DESCRIPTION OF DRAWINGS

[0029] Figure 1 The method flowchart of the application; Figure 2 The Gaussian mixture sampling strategy of the application; Figure 3 The adaptive step length adjustment diagram of the application; Figure 4 The hierarchical collision detection flowchart of the application; Figure 5 The dynamic iteration termination mechanism diagram of the application; Figure 6 The RRT*-GSQ path planning diagram of the application; Figure 7 The obstacle-free final effect and the obstacle-containing final effect diagram of the application. DETAILED DESCRIPTION

[0030] With reference to the drawings of the embodiments of the present application, the technical solutions in the embodiments of the present application will be clearly and completely described, obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work are within the protection scope of the present application.

[0031] Embodiments: Please refer to the drawings of the embodiments of the present application, the technical solutions in the embodiments of the present application will be clearly and completely described, obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work are within the protection scope of the present application. Figure 1 -Appendix Figure 5 , the embodiments of the present application provide a hybrid sampling global path planning method for complex orchard scenes, comprising the following steps: S1, is the starting step of the iterative loop of the method, which aims to generate random sampling points for subsequent node expansion.

[0032] This step is realized by Gaussian mixture sampling strategy to balance between global exploration and key area guidance.

[0033] In step S1, first, a probability decision is made to determine the specific sampling method used in this iteration, the decision process includes: Generate a random number uniformly distributed in the interval , and compare it with the pre-set probability threshold as a method parameter, the threshold The value range is (0, 1).

[0034] When the random number is greater than the pre-set probability threshold , the method performs global uniform sampling, which generates random sampling points with uniform probability distribution in the feasible configuration space after removing the obstacle-occupied space in the entire configuration space , which is mathematically expressed as: ; Where, represents uniform distribution on the feasible configuration space , this operation ensures that the algorithm has the ability to explore the entire workspace, avoiding falling into local optimal solution due to local guidance.

[0035] When the random number is not greater than the pre-set probability threshold , the method performs Gaussian bias sampling, which concentrates the sampling points in one or more pre-set key areas, for example, near the center line of the inter-row passage in the orchard environment, the sampling points are generated from the pre-set parameter Gaussian distribution, which is mathematically expressed as: ; In this expression, is the generated sampling point; is the mean vector of Gaussian distribution, whose value is set as the geometric center coordinate of the key region, to position the expected location of the sampling in the narrow passage or the like; is the covariance matrix, which defines the distribution shape and range of the sampling point around the mean vector.

[0036] In the specific embodiment of the two-dimensional configuration space, the covariance matrix can be expressed as: ; wherein, and are the standard deviations of the sampling point in the axis and axis directions, respectively, by setting different and values, the shape of the Gaussian sampling region can be controlled, for example, in the narrow passage along the axis direction, a larger and a smaller can be set, so that the sampling points are more distributed along the passage direction, and are more concentrated in the direction perpendicular to the passage.

[0037] Through the above hybrid mechanism, the output of step S1 is the random sampling point which retains randomness and is guided to the key region beneficial to pathfinding according to probability, the probability threshold , the mean of Gaussian distribution and the covariance matrix are all method parameters that can be pre-configured according to the specific working environment (such as passage width, robot size).

[0038] After obtaining the random sampling point through step S1, step S2, i.e., dynamic node expansion, is performed, the purpose of this step is to expand the dynamically adjusted step length from the existing nodes in the random tree in the direction of , thereby generating a new node .

[0039] First, to determine the starting point of expansion, the node closest to the sampling point in the random tree needs to be found, which is achieved by traversing all existing nodes in the random tree and calculating the distance between each node and the sampling point The Euclidean distance is realized, and the node with the minimum distance is determined as the nearest node .

[0040] Next, adaptive step adjustment is performed to determine the extended distance, the extended step is not a fixed value, but is dynamically calculated according to the distance between the nearest node and the global target point . The calculation formula is as follows: ; wherein, is the step length adopted this time; is a preset minimum step length parameter, used to ensure that there is still basic expansion capability when approaching the target point; is a preset maximum step length parameter, used to limit the distance of a single expansion, preventing the obstacle from being crossed or the key path from being missed; is a preset proportion coefficient; is the target point of global path planning; is the nearest node determined as described above; represents the Euclidean distance between two points.

[0041] After determining the extended step, direction optimization guidance is performed to determine the direction of the extension, which is not simply directed to , but is composed by two direction vectors weighted. First, two unit direction vectors are defined: a direction vector towards the randomly sampled point ; a direction vector towards the global target point ; Subsequently, the obstacle influence factor is calculated, which is calculated according to the distance between and the nearest obstacle in the environment, and the calculation method is a piecewise function: ; wherein, is the shortest distance between and the surface of the nearest obstacle; is a preset safety distance threshold, when is greater than , is 0; when is less than or equal to , the value of increases linearly from 0 to 1 as decreases.

[0042] Final extension direction vector By And Through Weighted summation is obtained, and normalization processing is performed to obtain a unit direction vector : ; ; Finally, combined with the calculated step size and direction , the coordinates of the new node are determined: ; In an embodiment, to further optimize the path quality, step S2 also includes a multi-candidate node generation and selection mechanism. After calculating the basic extension direction , a plurality of candidate directions with slight angular deviations can be generated around the direction, and a group of candidate new nodes are generated accordingly. Then, for each candidate node, the cumulative path cost of reaching the node from the path starting point via the random tree is calculated, and finally, from the group of candidate nodes, the one with the smallest path cost is selected and determined as the final new node to enter the subsequent collision detection step.

[0043] After generating the candidate new node in step S2, the method performs step S3, i.e., hierarchical collision detection, which is used to determine whether the path segment connecting the parent node and the new node collides with any obstacle in the environment. This step is implemented through a three-stage process consisting of coarse detection, secondary screening, and fine detection to reduce computational complexity.

[0044] Stage 1: Coarse detection based on quadtree In the initialization phase of the method, a quadtree spatial index needs to be constructed for all static obstacles in the entire working environment. Quadtree is a tree-like data structure that recursively divides a two-dimensional space region into four equal sub-quadrants. This process continues until the number of obstacles contained in each leaf node is less than a pre-set threshold, or the depth of the tree reaches a pre-set maximum value. Each leaf node stores the index of all obstacles contained in the spatial region it represents.

[0045] When collision detection is needed, first determine the path segment to be detected (from and The space range of the defined line segment is then queried to determine which leaf nodes in the quadtree represent a space region that intersects with the path segment. Through this query operation, all obstacle sets that may overlap with the path segment in space can be quickly located, and all leaf nodes and their contained obstacles that do not intersect with the region where the path segment is located are directly excluded without entering a subsequent detection process.

[0046] Stage Two: Secondary Screening Based on Axis-Aligned Bounding Box (AABB) For the candidate obstacle set screened through stage one, secondary screening is performed. In this stage, axis-aligned bounding box (AABB) is used for detection. AABB is the smallest rectangle that encloses a geometric body, with each side parallel to the coordinate axis. In the preprocessing stage, the AABB of each obstacle in the environment has been calculated and stored, and the AABB of the path segment to be detected is also dynamically calculated.

[0047] The operation in this stage is to traverse each obstacle in the candidate obstacle set and determine whether the AABB of the obstacle overlaps with the AABB of the path segment. The intersection detection of AABB has a very small calculation amount, and only needs to determine the overlap of the projection interval of each coordinate axis. Only when the AABB of the obstacle overlaps with the AABB of the path segment, the obstacle is considered to pass the secondary screening and needs to be finally accurately detected. Obstacles that do not overlap are excluded.

[0048] Stage Three: Accurate Geometric Collision Judgment For the obstacles screened through the previous two stages, the number of which has been greatly reduced, the final accurate geometric collision judgment is performed. In this stage, the operation is directly performed between the accurate geometric model (such as a polygon) of the obstacle and the path segment (line segment).

[0049] The specific detection algorithm can use the Separating Axis Theorem or the edge-by-edge intersection test of a line segment and a polygon. For example, when using the edge-by-edge intersection test, the path segment to be detected is regarded as a line segment, and the obstacle is regarded as a polygon composed of multiple edges. Then, each edge of the polygon is traversed, and it is determined whether the edge intersects with the line segment. As long as any edge is found to intersect with the path segment, it is determined that a collision occurs, and all subsequent detections of the path segment are immediately terminated. The node expansion is marked as a failure. If all the edges of all the screened obstacles are traversed and no intersection is found, it is determined that the path segment has no collision, and the new node is effective.

[0050] Through the three-stage hierarchical filtering described above, step S3 concentrates the calculation amount of collision detection on a small number of obstacles that are most likely to collide, thereby achieving acceleration of the entire process.

[0051] Step S4 is the iteration termination control mechanism of the method. It is not executed in every iteration, but activated after certain preconditions are met, to determine whether the whole path planning algorithm should terminate execution and output the result. The purpose of this mechanism is to balance the path quality and the computation time.

[0052] The judgment logic of this dynamic iteration termination mechanism consists of two parallel conditions. Only when both conditions are met at the same time, the algorithm will terminate.

[0053] Condition 1: Minimum iteration number reached To ensure the random tree has enough time to explore the configuration space and avoid getting a suboptimal solution due to premature convergence, the method sets a minimum iteration number In the early stage of algorithm execution, even if a feasible path has been found and its quality no longer changes, as long as the current total iteration number has not reached , the termination mechanism will not be triggered. That is, the precondition for termination judgment is: ; where is a positive integer parameter set in advance according to the complexity of the environment and the accuracy requirement of the solution.

[0054] Condition 2: Path quality convergence Once the total iteration number of the algorithm meets Condition 1, the termination mechanism starts to activate and monitor the quality of the current optimal path to determine whether it has converged. This process first needs to quantitatively evaluate the path quality.

[0055] In the embodiment, the path quality is a comprehensive index composed of the weighted sum of multiple sub-indices. Its calculation formula is: ; where denotes the current optimal path; is the total length of the path; is the smoothness measure of the path, for example, it can be the inverse of the sum of the curvatures of all the nodes on the path; is the safety measure of the path, i.e., the minimum distance between all points on the path and the nearest obstacle; , , are the weight coefficients of the corresponding sub-indices, which are preset parameters used to adjust the importance of different performance indicators in the overall quality evaluation.

[0056] In each iteration, if a better path is found (i.e., the path quality (If the path cost is higher or lower), the algorithm updates and records the current optimal path and its quality. The convergence state is determined by the algorithm maintaining and recording the most recent... In each iteration, the historical window of the optimal path quality value at the end of each iteration is used to calculate the current optimal path quality after condition one is met. Compared to the quality of the optimal path in the previous iteration relative improvement rate : ; If continuous The relative improvement rate calculated in the next iteration All are less than the preset convergence threshold If the path quality has reached convergence, then the path quality is considered to have converged. and All are adjustable preset parameters.

[0057] Finally, the termination logic for step S4 is as follows: Current iteration number When both the sub-conditions of path quality reaching convergence are met, the entire iteration process terminates. After the process terminates, the method outputs the currently stored optimal path as the final planning result. If either sub-condition is not met, the process returns to step S1 and continues to execute the next iteration.

Claims

1. A hybrid sampling global path planning method for complex orchard scenes, characterized in that, comprising the following steps: S1, adopting a Gaussian mixed sampling strategy to generate random sampling points in the configuration space; S2, adopting a node expansion strategy of dynamic step length and direction optimization to select the nearest node from the random tree to expand in the direction of the random sampling point to generate a new node; S3, adopting a collision detection strategy of quadtree combined with AABB to perform collision detection on the new node and the expansion path; S4, adopting a dynamic iteration termination mechanism driven by path quality evaluation to determine whether to terminate the algorithm and output the optimal path. The Gaussian mixed sampling strategy in step S1 specifically includes:

2. The hybrid sampling global path planning method for complex orchard scene according to claim 1, wherein, Setting a random threshold, when the generated random number meets the condition, generating random sampling points in the pre-set key area using Gaussian distribution; otherwise, generating random sampling points in the global range using uniform distribution. The node expansion strategy of dynamic step length and direction optimization in step S2 includes adaptive step length adjustment:

3. The hybrid sampling global path planning method for complex orchard scene according to claim 1, wherein, According to the distance from the current expansion node to the target, dynamically set the expansion step length, take a larger step length when the distance is far, and reduce the step length when close to the target. The node expansion strategy of dynamic step length and direction optimization also includes introducing direction optimization guidance:

4. The hybrid sampling global path planning method for complex orchard scene according to claim 3, wherein, Comprehensively consider the random sampling direction and the direction towards the target, and calculate the obstacle influence factor according to the distance from the current expansion node to the nearest obstacle, and take the final expansion direction as the weighted result of the random sampling direction and the target direction according to the obstacle influence factor. The collision detection strategy of quadtree combined with AABB in step S3 specifically includes:

5. The hybrid sampling global path planning method for complex orchard scene according to claim 1, wherein, First, construct a quadtree spatial index for static obstacles in the environment; When performing path segment collision detection, use the quadtree to quickly locate the area node where the path segment is located, and only perform fine collision judgment for obstacles that have intersection with the axis-aligned bounding box (AABB) of the area node. The dynamic iteration termination mechanism driven by path quality evaluation in step S4 specifically includes:

6. The hybrid sampling global path planning method for complex orchard scene according to claim 1, wherein, Set the minimum iteration threshold and the path quality improvement threshold, when the iteration number of the algorithm reaches the minimum iteration threshold, and the path quality improvement of continuous iterations is lower than the path quality improvement threshold, the algorithm terminates. The path quality evaluation is a comprehensive evaluation of the path length, smoothness, and distance from obstacles of the current path.

7. The hybrid sampling global path planning method for complex orchard scene according to claim 6, wherein, The Gaussian distribution is arranged near the center of the inter-row passage in the orchard, and the narrow passage center is the mean value.

8. The hybrid sampling global path planning method for complex orchard scene according to claim 2, wherein, The calculation method of the obstacle influence factor is:

9. The hybrid sampling global path planning method for complex orchard scene according to claim 4, wherein, When the obstacle distance is greater than the safety threshold, it is set to 0, at this time the expansion is mainly towards the random sampling direction; when close to the obstacle, the factor increases, making the expansion direction gradually deviate towards the target direction. The node expansion strategy in step S2 also includes:

10. The hybrid sampling global path planning method for complex orchard scene of claim 1, wherein, Generate multiple candidate nodes in different directions each time, calculate the path cost of each candidate node, and select the candidate node with the smallest path cost as the new node to access the tree. ​

Images