An orchard navigation robot path planning method based on improved A* algorithm and DWA algorithm fusion

Abstract

The application belongs to the technical field of orchard navigation robots, and particularly relates to an orchard navigation robot path planning method based on fusion of improved A* algorithm and DWA algorithm, aiming at improving the path planning of the orchard navigation robot. The method comprises the following steps: obtaining orchard three-dimensional point cloud data based on a three-dimensional laser radar, and constructing a two-dimensional grid map. An improved A* algorithm adopting a rectangular expansion search strategy is used to generate a global path containing key nodes based on the two-dimensional grid map. The global path is smoothed through a dynamic tangent circle strategy to obtain a smooth path with continuous curvature. An improved DWA algorithm is fused to perform local obstacle avoidance planning based on the smooth path.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of orchard navigation robots, and particularly relates to an orchard navigation robot path planning method based on fusion of improved A* algorithm and DWA algorithm. BACKGROUND

[0002] With the advancement of agricultural modernization, orchard navigation robots are increasingly widely used in autonomous operation, and one of the core technologies is path planning, that is, a safe, efficient and easy-to-track path for robots is planned from the starting point to the end point in the unstructured orchard environment with dense fruit trees and complex terrain.

[0003] In the prior art, orchard path planning is mostly based on sensors to obtain environmental data to construct a map, and combined with A* algorithm for global path planning, and DWA algorithm for local obstacle avoidance. However, the traditional method has many shortcomings in practical application: in the aspect of environment modeling, the grid map constructed based on laser radar point cloud data often has incomplete filtering of ground points and invalid points or poor noise processing, resulting in inaccurate obstacle identification and affecting the reliability of path planning. In global path search, the eight-neighborhood search strategy of traditional A* algorithm needs to traverse a large number of nodes, the search efficiency is low, and the generated path nodes are redundant, which is difficult to match the motion characteristics of the robot and has poor adaptability. SUMMARY

[0004] The purpose of the present application is to provide an orchard navigation robot path planning method based on fusion of improved A* algorithm and DWA algorithm, aiming to improve the path planning of the orchard navigation robot.

[0005] To achieve the above purpose, the technical scheme adopted by the present application is as follows: the present application provides an orchard navigation robot path planning method based on fusion of improved A* algorithm and DWA algorithm, characterized in that it comprises: S1: constructing a two-dimensional grid map based on three-dimensional point cloud data of an orchard obtained by a three-dimensional laser radar. S2: generating a global path containing key nodes based on the two-dimensional grid map by using an improved A* algorithm with a rectangular expansion search strategy. S3: smoothing the global path by a dynamic tangent circle strategy to obtain a smooth path with continuous curvature. S4: fusing an improved DWA algorithm to plan local obstacle avoidance based on the smooth path.

[0006] The method provided by the embodiments of the application can obtain point cloud data by a three-dimensional laser radar and construct a two-dimensional grid map, and can accurately reflect the distribution of obstacles such as fruit trees and terrain in an orchard by removing ground points, invalid points and noise, thereby avoiding path planning deviation caused by environmental model errors and ensuring the reliability of subsequent global path search and local obstacle avoidance. An improved A* algorithm using a rectangular expansion search strategy is used, a passable rectangle is used as a unit, only boundary nodes are accessed, the number of nodes to be traversed is greatly reduced, and the global path search speed is improved. At the same time, through key node selection rules, the generated global path is more in line with the motion characteristics of the robot. A dynamic tangent circle strategy is used to smooth the global path, the tangent circle is dynamically adjusted to avoid collision with obstacles, the path curvature is continuous, the sharp corners or sudden changes in curvature that may exist in the traditional path are eliminated, the motion control difficulty of the robot is reduced, and the increase in energy consumption or the decrease in work precision caused by the non-smooth path is reduced. The improved DWA algorithm is fused to perform local obstacle avoidance planning, which can not only provide macroscopic navigation guidance based on the smoothed global path, but also generate reasonable local obstacle avoidance trajectories through an optimized evaluation function (taking into account global path consistency, obstacle avoidance, etc.), so as to ensure that the robot can quickly return to the global path after avoiding unknown obstacles, balance the path safety and work efficiency, and meet the complex needs of the unstructured environment of the orchard.

[0007] In some embodiments, constructing the two-dimensional grid map includes: removing ground points and invalid points higher than the working height in the three-dimensional point cloud data by using a ground plane fitting method. The remaining point cloud is projected onto the ground plane, and the grid units are divided according to a predetermined resolution. The number of points in the grid units is counted, and the two-dimensional grid map is generated after removing isolated noise units.

[0008] In some embodiments, generating a global path containing key nodes based on the two-dimensional grid map includes: taking a passable rectangle as a basic unit, generating a horizontal linear region along the obstacle boundary from a starting node, and extending to form a rectangular region. Screening boundary nodes that are not adjacent to obstacles as successor search nodes, and repeating the rectangular expansion to include a target node. Key nodes are selected according to a horizontal and vertical coordinate matching rule, and a global path is generated by connecting the key nodes.

[0009] In some embodiments, selecting key nodes according to a horizontal and vertical coordinate matching rule and generating a global path includes: adding the starting node to the key node list. The horizontal and vertical coordinates of the successor search node are judged, if the horizontal coordinates of the successor search node are the same, the vertical coordinate of the central one is selected. If the vertical coordinates are the same, the one with the smallest Euclidean distance to the target node is selected. After including the target node, it is added to the list, and the global path is formed by connecting the key nodes.

[0010] In some embodiments, the global path is smoothed by a dynamic tangent circle strategy, including: taking a key node as a control point, and constructing a tangent circle at the tangent point of adjacent shorter line segments. Detecting the collision of the circle arc with the obstacle, dynamically adjusting the tangent point, and reconstructing the tangent circle until there is no conflict. Replacing the line segment with the circle arc to realize the continuous curvature of the global path.

[0011] In some embodiments, the improved DWA algorithm is fused, including optimizing the evaluation function, which includes: based on the key path node of the improved A* algorithm as a reference, calculating the azimuth angle deviation of the end of the simulated trajectory. Calculate the deviation distance of the simulated trajectory from the global path, and the distance from the known and / or unknown obstacles. The evaluation indexes are weighted and fused by weight coefficients and smoothing coefficients.

[0012] In some embodiments, local obstacle avoidance planning is performed based on the smoothed path, including: predicting multiple groups of motion trajectories by sampling the combination of linear velocity and angular velocity. Based on the optimized evaluation function, the optimal trajectory is selected to control the robot to perform obstacle avoidance action. After the obstacle avoidance is completed, return to the smooth path to continue navigation.

[0013] In some embodiments, the invalid points are point cloud data higher than the working height of the robot.

[0014] In some embodiments, the evaluation indexes of the smoothing process include at least one of path length, planning time, search node number, number of turns, and curvature continuity.

[0015] In some embodiments, the evaluation process of the smoothing process includes: under the same starting point and target point conditions, recording the path length, average planning time, search node number, path node number, number of turns, cumulative turning angle, and curvature mutation of the path generated by each algorithm in different scale grid maps. Calculate the average value of the path length, average planning time, search node number, path node number, number of turns, cumulative turning angle, and curvature mutation by multiple iterations. Based on the average value, evaluate the path efficiency, smoothness, and safety. BRIEF DESCRIPTION OF DRAWINGS

[0016] Figure 1 is a schematic diagram of a path planning method for orchard navigation robots based on the fusion of improved A* algorithm and DWA algorithm provided by the embodiments of the present application;

[0017] Figure 2 is a process schematic diagram for constructing a grid map provided by the embodiments of the present application;

[0018] Figure 3 is a schematic diagram of a rectangular expansion strategy provided by the embodiments of the present application;

[0019] Figure 4is a schematic diagram of a dynamic pressure tangent circle strategy provided by an embodiment of the present application;

[0020] Figure 5 is a schematic diagram of performance comparison of different algorithm paths provided by an embodiment of the present application;

[0021] Figure 6 is a schematic diagram of performance comparison of different algorithms provided by an embodiment of the present application;

[0022] Figure 7 is a schematic diagram of an orchard simulation environment provided by an embodiment of the present application;

[0023] Figure 8 is a schematic diagram of a track-type robot chassis and a model diagram provided by an embodiment of the present application;

[0024] Figure 9 is a schematic diagram of an experiment simulation provided by an embodiment of the present application;

[0025] Figure 10 is another schematic diagram of an experiment simulation provided by an embodiment of the present application. DETAILED DESCRIPTION

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

[0027] In the description of the application, it should be understood that the terms "upper", "lower", "left", "right", "front", "back", "inner", "outer" and the like indicate the orientation or positional relationship based on the orientation or relative position relationship shown in the drawings, and are only for the purpose of facilitating the description of the present application and simplifying the description, and do not indicate or imply that the device or element referred to must have a particular orientation, be constructed and operated in a particular orientation, and therefore cannot be understood as a limitation on the present application. Unless otherwise specified, the above directional description can be flexibly arranged in the process of actual application, under the condition of meeting the relative position relationship shown in the drawings.

[0028] The terms "first", "second" are only for descriptive purposes, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined with "first", "second" can explicitly or implicitly include one or more of the features. In the description of the present application, unless otherwise specified, the meaning of "multiple" is two or more.

[0029] In the description of the present application, it should be noted that unless specifically defined and limited, the terms "mounting", "connection", "connecting", "communication" should be understood broadly, for example, it can be fixed connection, or detachable connection, or integral connection. It can be direct connection, or indirect connection through intermediate medium, or internal communication of two elements. For those skilled in the art, the specific meaning of the above terms in the present application can be understood according to the specific circumstances.

[0030] In the embodiments of the present application, the terms "comprising", "containing" or any other variants thereof are intended to cover non-exclusive inclusion, so that the process, article or device including a series of elements not only includes those elements, but also includes other elements not explicitly listed, or includes elements inherent to such process, article or device. Without more limitation, the element defined by the sentence "including a…" does not exclude the presence of other identical elements in the process, article or device including the element.

[0031] In the embodiments of the present application, the words "exemplary" or "for example" are used to mean serving as an example or illustration. Any embodiment or design presented as "exemplary" or "for example" in the embodiments of the present application should not be interpreted as being more preferred or advantageous than other embodiments or design solutions. Rather, the use of "exemplary" or "for example" is intended to present concepts in a concrete manner.

[0032] As shown in the background, there is an urgent need for a path planning method that can optimize the accuracy of environmental modeling, improve the efficiency and adaptability of global path search, enhance path smoothness, and achieve efficient fusion of global and local planning, to meet the needs of orchard navigation robots for path safety, efficiency and motion controllability.

[0033] Therefore, the embodiments of the present application provide an orchard navigation robot path planning method based on the fusion of improved A* algorithm and DWA algorithm. For example, as shown in the Figure 1 The method comprises the following steps:

[0034] S1: Based on the three-dimensional point cloud data of the orchard obtained by the three-dimensional laser radar, a two-dimensional grid map is constructed.

[0035] Step S1 aims to provide an accurate environmental model for the navigation robot. In some embodiments, constructing a two-dimensional grid map includes: using a ground plane fitting method to remove ground points and invalid points above the working height in the three-dimensional point cloud data. Project the remaining point cloud onto the ground plane, divide the grid cells according to the predetermined resolution, map the projected points into the cells and count the number of points in the cells, remove the isolated noise cells, and then determine whether there is an obstacle in the cell according to the number of points in the cell, thereby generating a two-dimensional grid map with clear structure and accurate obstacle distribution.

[0036] In some embodiments, the invalid points are point cloud data higher than the robot working height.

[0037] As shown in the drawings, Figure 2 After obtaining the three-dimensional point cloud data of the orchard by the three-dimensional laser radar, the ground plane fitting method is used to remove the ground and the invalid points above the robot working height, and the effective points are projected onto the plane by ignoring the height information (point cloud projection sub-step in Figure 2 ), and a grid unit is generated according to the predetermined resolution (grid creation sub-step in Figure 2 ), the number of projected points is counted and isolated noise units are removed (noise removal sub-step in Figure 2 ). According to the diameter of the fruit trees and the length of the tree rows, the tree rows are expanded and extended to the walls to build an "arch" shaped working corridor, and a safety expansion area is generated around the fruit trees and walls, and finally a standardized orchard environment grid map is obtained (state determination sub-step in Figure 2 ). In the Gazebo simulation platform, an orchard simulation environment is built, and the Komodo-02 tracked robot chassis is equipped with a radar sensor to obtain environmental information, and a map is constructed to verify the effectiveness of the fusion algorithm.

[0038] By removing the ground points by ground plane fitting, the terrain undulations are eliminated, the actual obstacles such as fruit trees are focused on, the invalid points (such as branches and leaves) higher than the working height are filtered out, and the influence of irrelevant data on map construction is avoided, so that the map is more suitable for the actual working space requirements of the robot. Count the number of points in the unit and remove isolated noise units to effectively filter out false obstacle points caused by sensor errors or environmental interference, effectively enhancing the reliability of the map.

[0039] S2: Improved A* algorithm with rectangular expansion search strategy, generating a global path containing key nodes based on a two-dimensional grid map.

[0040] In some embodiments, generating a global path containing key nodes based on a two-dimensional grid map includes:

[0041] Taking the passable rectangle as the basic unit, a horizontal linear area is generated along the obstacle boundary from the starting node, and a rectangular area is formed by extension.

[0042] Filtering the boundary nodes that are not adjacent to obstacles as successor search nodes, and repeating the rectangular expansion to include the target node.

[0043] Selecting key nodes according to the horizontal and vertical coordinate matching rules, and connecting to generate a global path.

[0044] As shown in the drawings, Figure 3As shown, the rectangular expansion search strategy uses a walkable rectangle (basic unit). First, an initial linear region is generated horizontally from the starting node along the obstacle boundary. Then, this linear region is extended vertically to form the first rectangular region. Figure 3 The initial rectangular region is defined, and boundary nodes not adjacent to obstacles are selected as candidate successor search nodes based on the relative positions of the rectangle's sides and the obstacles. The optimal node is used to determine a new linear region, and the rectangle expansion is repeated along the corresponding direction until the target node is contained within a rectangular region. Because this application improves the eight-neighborhood search of the A* algorithm to a rectangle expansion search, only visiting the boundary nodes of the rectangle, it enables fast and efficient node search.

[0045] In some embodiments, key nodes are selected according to the matching rules of horizontal and vertical coordinates, and a global path is generated by connecting them. This includes: adding the starting node to the key node list; judging the horizontal and vertical coordinates of subsequent search nodes; if the horizontal coordinates of subsequent search nodes are the same, selecting the one with the middle vertical coordinate; if the vertical coordinates are the same, selecting the one with the smallest Euclidean distance to the target node; including the target node and adding it to the list, and connecting them to form the global path. "Including the target node and adding it to the list" means that when the extended rectangle includes the target node, the target node is also added to the list.

[0046] When successor nodes have the same x-coordinate, selecting the one with the middle y-coordinate reduces frequent horizontal path deviations, making the lines connecting nodes in the same column closer to straight lines and lowering the robot's turning frequency in that direction. When y-coordinates are the same, selecting the one with the smallest Euclidean distance to the target node guides the path towards the target direction, avoiding redundant detours, shortening the path length, and reducing ineffective turns, making the global path more aligned with the requirement of "efficient direct access".

[0047] The rule of "adding the target node to the list when the extended rectangle contains it" ensures that the target node is always the endpoint of the path, avoiding the problem of the path not covering the target point due to search deviation. The key nodes from the starting node to the target node are connected sequentially to form a continuous and unbroken global path, ensuring that the robot can stably reach the target point from the starting point, meeting the full-process navigation requirements of "start point - work point - end point" in orchard operations.

[0048] S3: The global path is smoothed by a dynamic tangent circle strategy to obtain a smooth path with continuous curvature.

[0049] For example, such as Figure 4 As shown, in some embodiments, a dynamic tangent circle strategy is used to smooth the global path, including:

[0050] Using key nodes as control points, construct tangent circles at the tangent points of the shorter segments of adjacent polyline segments.

[0051] Detect the collision between the arc and the obstacle, dynamically adjust the tangent point, and reconstruct the tangent circle until there is no conflict.

[0052] More specifically, if a collision occurs, the tangent point is dynamically moved along the original polyline segment direction to reconstruct the tangent circle and re-detect until the arc and the environment have no conflict.

[0053] Replace the polyline segment with an arc to achieve continuous curvature of the entire path.

[0054] The curvature-continuous path makes the robot's speed and angular velocity adjustment more coherent, reducing the sudden changes in motion parameters (such as the instantaneous parameter jump from straight to sharp turn), thereby reducing the difficulty of control system debugging. At the same time, the smoothed path provides a more stable "baseline trajectory" for subsequent local obstacle avoidance (such as trajectory prediction in the DWA algorithm), making the local planning less likely to deviate due to the dramatic fluctuations of the global path, and improving the synergy of global and local planning.

[0055] Figure 4 A-B-C in the above figure is the global polyline path after key node screening, and AB and BC are adjacent polyline segments. The strategy first identifies the shorter segment (using AB as an example), takes tangent points M and N on AB and BC respectively with B as the control point, constructs a tangent circle with center O2 and radius R2, and replaces the segment BN with the arc MN. If the constructed arc MN collides with the obstacle, the strategy moves the tangent points along the original polyline segments AB and BC (e.g., adjusts M to the left of A and N to the right of C), reconstructs a tangent circle with a new center O1 and radius R1, and continues until there is no collision.

[0056] S4: Fuse the improved DWA algorithm to perform local obstacle avoidance planning based on the smoothed path.

[0057] In some embodiments, the improved DWA algorithm is fused, including optimizing the evaluation function, which includes calculating the azimuth deviation of the end of the simulated trajectory based on the key path nodes of the improved A* algorithm as a reference. Calculate the deviation distance of the simulated trajectory from the global path and the distance from known and / or unknown obstacles. Weight and fuse the individual evaluation indicators through weight coefficients and smoothing coefficients.

[0058] In some embodiments, local obstacle avoidance planning based on the smoothed path includes predicting multiple sets of motion trajectories by sampling combinations of linear and angular velocities. Based on the optimized evaluation function, the optimal trajectory is selected to control the robot to perform obstacle avoidance actions. After the obstacle avoidance is completed, the robot returns to the smoothed path to continue navigation.

[0059] As a possible implementation, after sampling multiple sets of linear velocity and angular velocity and predicting the motion trajectory, the azimuth evaluation in the original evaluation function based on the target node is changed to be referenced to the key path node obtained by the improved A* algorithm, an evaluation item of the deviation distance of the simulated trajectory from the global planning path is added, and different distance evaluation functions are set for static known obstacles and unknown obstacles. Finally, the evaluation indexes are weighted and fused by weight coefficients and normalization smoothing coefficients, so as to both fit the global optimal path and ensure smoothness and safety in local real-time obstacle avoidance. The finally modified evaluation function is:

[0060]

[0061] In the formula, is the azimuth evaluation function, indicating the angular deviation of the end of the simulated trajectory from the position of the key path node, and the smaller the angular deviation, the greater the value; is the path deviation distance evaluation function, indicating the distance between the simulated trajectory and the global planning path, and the smaller the distance, the greater the value; is the nearest distance between the end of the simulated trajectory and the known obstacle; is the nearest distance between the end of the simulated trajectory and the unknown obstacle; is the speed evaluation function, indicating the speed of the simulated trajectory of the selected speed group, and the greater the speed, the greater the value; is the smoothing coefficient; , , , , is the weight coefficient of the corresponding evaluation function.

[0062] In some embodiments, the evaluation indexes of the smoothing processing include at least one of path length, planning time, search node number, turn angle number, and curvature continuity.

[0063] As a possible implementation, the evaluation process of the smoothing processing includes: under the same starting point and target point conditions, recording the path length, average planning time, search node number, path node number, turn angle number, cumulative turning angle, and curvature mutation of the path generated by each algorithm in different scale grid maps. The path length, average planning time, search node number, path node number, turn angle number, cumulative turning angle, and curvature mutation are iteratively calculated and averaged. The path efficiency, smoothness, and safety are evaluated based on the average values.

[0064] The present application aims at the problems of traditional path planning algorithm in orchard scene, such as too many node traversals, long search time, poor path smoothness and proximity to obstacles, etc. On the basis of traditional A* algorithm, the rectangular expansion search strategy, key node selection scheme and path smoothing method based on dynamic tangent circle are introduced to generate a global navigation path that meets the operation requirements of orchard environment. The evaluation function of traditional DWA algorithm is optimized, a global path deviation distance index is added, and the azimuth and obstacle evaluation function is improved, so as to assist the mobile robot to select a better local motion trajectory. The improved A* algorithm and DWA algorithm are combined to realize the cooperative planning of global navigation and local obstacle avoidance.

[0065] The present application also provides experimental verification and performance evaluation of the method, for example:

[0066] Using the configuration of Windows 11 operating system, Intel Core i5 processor, 3.10 GHz main frequency, 16 GB running memory and Python 3.10 software environment, the traditional A* algorithm, other improved algorithms and the method provided by the present application are selected for testing.

[0067] In order to roughly simulate the working environment of the robot, the environment maps of 37x25 and 57x41 are constructed according to the grid method, the same starting point and target point are set, and the performance evaluation indexes of path length, average planning time, search node number, path node number, corner number, turning angle and whether there is curvature mutation of different algorithms are recorded. A* algorithm is an optimal search algorithm, so its output path is unique, but the calculation time is different in single iteration due to the comprehensive influence of system test environment and hardware performance. In order to eliminate the random influence of calculation time, each algorithm is tested ten times under the same test conditions, and the average planning time of each algorithm is calculated to fully verify the calculation efficiency of the improved algorithm. The performance comparison of different algorithm paths is shown in Figure 5 .

[0068] Figure 5The data show that, in the 37x25 grid map, compared with the traditional A* algorithm, the weighted improved A* algorithm is consistent in the length of the planned path and the number of path nodes, the number of search nodes and the planning time is reduced, but the number of turning points and the turning angle is increased, and the path has curvature mutation. After the path of the weighted improved A* algorithm is smoothed by the Bezier curve algorithm, the path length is reduced, but the planning time is slightly increased, and the smoothed path does not have curvature mutation. The path length planned by the improved A* algorithm provided in the embodiment is greater than that of the traditional A* algorithm and the weighted improved A* algorithm, which is mainly consumed in ensuring the rationality of the path, and is the effect of increasing the path length for the robot to perform picking work on the fruit trees. Except for this performance reduction, the path planning time, the number of search nodes, the number of path nodes, the number of turning points and the turning angle of the improved algorithm in the experiment are less than those of the traditional A* algorithm and the weighted improved A* algorithm.

[0069] Compared with the traditional A* algorithm, the search node number of the improved algorithm path provided in the application is reduced by 42.5%, the path node number is reduced by 90.3%, the turning point number is reduced by 58.3%, and the cumulative turning angle is reduced by 15.4%, but there is a problem of path curvature mutation. After the path is optimized using the path smoothing method provided in the application, the path length is reduced and the planning time is increased, compared with the traditional A* algorithm, the path length is increased by 11.2%, the planning time is reduced by 20.1%, and the curvature mutation problem of the polyline path is eliminated.

[0070] In the 57x41 grid map, compared with the traditional A* algorithm, the path length of the improved algorithm provided in the application is increased by 7.8%, the planning time is reduced by 31.0%, the search node number of the path is reduced by 50.9%, the path node number is reduced by 94.4%, the turning point number is reduced by 65.4%, the cumulative turning angle is reduced by 30.2%, and the whole path curvature is continuous. In summary, the improvement strategy proposed in the application for the A* algorithm has obvious advantages, can effectively complete the path planning task of the robot, although the path length is sacrificed, but the path trajectory is more suitable for the picking work route of the robot in the orchard environment, and other performance indicators are optimized to different degrees.

[0071] The embodiment of the application also provides fusion algorithm experiment verification, and the performance is evaluated according to the experimental results, and the experiment and the evaluation are as follows:

[0072] In order to verify the obstacle avoidance ability of the mobile robot after fusing the DWA algorithm, unknown obstacles are added to the path obtained by the improved A* planning algorithm. Ten simulation experiments are carried out in 37x25 and 57x41 environments respectively, the average path length and planning time of different fusion algorithms are calculated, and the performance comparison of the algorithms is as shown in Figure 6 .

[0073] In the environment of 37x25, the path length planned by the improved A* algorithm combined with the traditional DWA algorithm provided in the present application is 89.31 m, and the planning time is 25.82 s. The path length planned by the improved A* algorithm combined with the improved DWA algorithm provided in the present application is 88.27 m, and the planning time is 22.31 s. The improvement degrees of the path length and the planning time are 1.16% and 13.59%, respectively. In the environment of 57x41, the path length planned by the improved A* algorithm combined with the traditional DWA algorithm provided in the present application is 226.09 m, and the planning time is 61.85 s. The path length planned by the improved A* algorithm combined with the improved DWA algorithm provided in the present application is 224.95 m, and the planning time is 57.29 s. The improvement degrees of the path length and the planning time are 0.5% and 7.37%, respectively. After the improved DWA algorithm provided in the present application is combined, the length and the time of the planned path are effectively shortened, the obstacle avoidance capability of the mobile robot in the orchard environment when facing unknown obstacles is realized, and the demand of the mobile robot in the orchard for realizing the picking operation walking route is more met.

[0074] The orchard scene and the robot model are built in the Gazebo simulation platform, and the two-dimensional grid map of the orchard environment is obtained based on the environment modeling strategy. For example, as shown in Figure 7 . The related components are added in the Rviz tool to display the grid map and the robot model. For example, as shown in Figure 8 .

[0075] The static unknown obstacles are added in the original orchard environment to test the navigation and obstacle avoidance effect of the combined algorithm. For example, as shown in Figure 9 and Figure 10 . The black outer frame in Figure 9 represents the orchard fence obstacle, and the internal black points represent the orchard trunk obstacle. The gray area represents the passable area of the robot in the orchard, the white area represents the local map used for obstacle avoidance of the robot, the red area represents the continuous detection of the surrounding environment by the laser radar for obstacle avoidance, and the robot model is located at the starting position in the lower left corner. The red area around the robot represents the orientation of the robot.

[0076] Figure 10 is based on Figure 9 , unknown static obstacles are added to the travel route of the robot in the orchard environment to test and verify the obstacle avoidance effect of the mobile robot. In Figure 9Under the shown orchard environment, the mobile robot first performs global path planning based on the improved A* algorithm, and obtains the green route shown in Fig. (a). In the process of driving along the planned route, the robot detects the unknown obstacle in front based on the radar information, dynamically adjusts the local route using the optimized DWA algorithm, as shown in Fig. (b). In the process of continuous advance, the position information of the obstacle is continuously detected, the driving speed and the advancing angle are corrected in real time, and the obstacle avoidance function is realized through the planned local route, as shown in Fig. (c), the robot safely rounds the obstacle from the left side, and keeps a reasonable distance from the obstacle. After passing through the added unknown obstacle, the robot continues to move to the global path, as shown in Fig. (d). The comprehensive results show that the improved A* and DWA fusion path planning algorithm proposed in this study enables the robot to have the ability to avoid static unknown obstacles in the process of moving along the global optimal path, verifying the feasibility of the proposed fusion algorithm.

[0077] In the description of the present specification, specific features, structures, materials or characteristics can be combined in any one or more embodiments or examples in a suitable manner.

[0078] The above is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto, and any person skilled in the art can easily think of changes or replacements within the technical range disclosed by the present application, which should be covered within the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A path planning method for an orchard navigation robot based on the fusion of an improved A* algorithm and a DWA algorithm, characterized in that, include: S1: Construct a two-dimensional grid map based on the three-dimensional point cloud data of the orchard acquired by the three-dimensional LiDAR; S2: An improved A* algorithm employing a rectangular expansion search strategy generates a global path containing key nodes based on the two-dimensional grid map; the generation of the global path containing key nodes based on the two-dimensional grid map specifically includes: Using a passable rectangle as the basic unit, a horizontal linear region is generated from the starting node along the boundary of the obstacle, which is then extended to form a rectangular region. Select boundary nodes that are not adjacent to obstacles as subsequent search nodes, and repeat the rectangle to expand to include the target node; Key nodes are selected according to the horizontal and vertical coordinate matching rules, and then connected to generate a global path; specifically, the selection of key nodes according to the horizontal and vertical coordinate matching rules and the generation of a global path includes: Add the starting node to the list of critical nodes; The x and y coordinates of the subsequent search nodes are compared. If the x coordinates of the subsequent search nodes are the same, the one with the middle y coordinate is selected; if the y coordinates are the same, the one with the smallest Euclidean distance to the target node is selected. After including the target node, add it to the list and connect them to form a global path; S3: The global path is smoothed using a dynamic tangent circle strategy to obtain a smooth path with continuous curvature; S4: A fusion-improved DWA algorithm is used for local obstacle avoidance planning based on the smooth path; the fusion-improved DWA algorithm includes optimizing the evaluation function, specifically including: Based on the critical path nodes of the improved A* algorithm, the azimuth deviation of the simulated trajectory's end direction is calculated; Calculate the deviation of the simulated trajectory from the global path, as well as the distance to known and / or unknown obstacles; The evaluation indicators are weighted and integrated using weighting coefficients and smoothing coefficients.

2. The path planning method for orchard navigation robots based on the fusion of the improved A* algorithm and the DWA algorithm as described in claim 1, characterized in that, The construction of the two-dimensional raster map includes: Ground points and invalid points above the working height in 3D point cloud data are removed using a ground plane fitting method. The remaining point cloud is projected onto the ground plane and divided into grid cells according to a predetermined resolution. After counting the number of points within a raster cell and removing isolated noise cells, a two-dimensional raster map is generated.

3. The path planning method for orchard navigation robots based on the fusion of the improved A* algorithm and the DWA algorithm as described in claim 1, characterized in that, The smoothing of the global path using a dynamic tangent circle strategy includes: Using key nodes as control points, construct tangent circles at the tangent points of the shorter segments of adjacent polyline segments; Detect the collision between the arc and the obstacle, dynamically adjust the tangent point, and reconstruct the tangent circle until there is no conflict; Replace line segments with arcs to achieve continuous curvature throughout the path.

4. The path planning method for orchard navigation robots based on the fusion of the improved A* algorithm and the DWA algorithm as described in claim 1, characterized in that, The local obstacle avoidance planning based on the smooth path includes: By sampling the combination of linear velocity and angular velocity, multiple sets of motion trajectories can be predicted; The optimal trajectory is selected based on the optimized evaluation function, and the robot is controlled to perform obstacle avoidance actions. After obstacle avoidance is completed, return to the smooth path to continue navigation.

5. The path planning method for orchard navigation robots based on the fusion of the improved A* algorithm and the DWA algorithm as described in claim 2, characterized in that, The invalid points are point cloud data that are higher than the robot's operating height.

6. The path planning method for orchard navigation robots based on the fusion of the improved A* algorithm and the DWA algorithm as described in claim 3, characterized in that, The evaluation metrics for the smoothing process include at least one of path length, planning time, number of search nodes, number of turns, and curvature continuity.

7. The path planning method for an orchard navigation robot based on the fusion of the improved A* algorithm and the DWA algorithm according to any one of claims 1-6, characterized in that, The evaluation process for the smoothing process includes: Under the same starting point and target point conditions, record the path length, average planning time, number of search nodes, number of path nodes, number of turns, cumulative turning angle, and curvature change of each algorithm in raster maps of different sizes. The average value of the path length, average planning time, number of search nodes, number of path nodes, number of turns, cumulative turning angle, and curvature change is calculated through multiple iterations. The path efficiency, smoothness, and safety are evaluated based on the average value.

Images