Improved ant colony dynamic path planning method and system

Abstract

The invention discloses an improved ant colony dynamic path planning method and system, and belongs to the technical field of robot path planning, and the method comprises the steps: obtaining the static environment information of an orchard, creating an orchard rasterized map, and formulating a global path based on a DIACO-GA algorithm; then, obtaining the pose information of the mobile robot and the information of the dynamic obstacle, determining the moving trajectory of the mobile robot and the dynamic obstacle, judging whether the mobile robot collides with the dynamic obstacle or not and the collision type, executing a corresponding obstacle avoidance strategy according to the collision type of different obstacles, and planning a local dynamic obstacle avoidance path; fusing the global path and the local dynamic obstacle avoidance path, and correcting the global path in real time; through the method, the optimal path from the starting point to the target point can be flexibly planned, and static obstacles and dynamic obstacles are effectively avoided.

Description

Technical Field

[0001] The present invention relates to the technical field of robot path planning, and in particular to an improved ant colony dynamic path planning method and system. Background Art

[0002] With the rapid development of artificial intelligence, mobile robots are widely used in industry, military and agriculture, and path planning algorithms are often used in mobile robots. In the agricultural field, path planning of mobile robots refers to the use of autonomous navigation technology and intelligent path planning algorithms to enable mobile robots to avoid obstacles in agricultural environments such as farmlands or orchards, and autonomously plan efficient, safe and low-energy operation paths.

[0003] At present, there are many studies on the path planning of orchard mobile robots in static environments in the prior art, that is, the global environmental information is known and there are no dynamic obstacles. The widely used path planning algorithms are mainly genetic algorithms (GA) and ant colony optimization (ACO) algorithms. The main feature of GA is to construct individual objects according to specific problems and then directly operate on individual objects. Therefore, it has inherent implicit parallelism and better global optimization capabilities, but the search is relatively blind. Due to the randomness of the algorithm, the optimization results also depend on probability. The ACO algorithm imitates the biological behavior of ants that rely on pheromones to communicate in nature. The ants behind will choose the walking route according to the pheromone concentration released on the path when the ants in front walk, that is: an optimization algorithm abstracted from the foraging behavior of ant colonies for solving optimization problems; it uses positive feedback mechanisms and historical search experience to improve optimization efficiency, but it is easy to fall into the disadvantage of local optimal solutions; since GA and ACO algorithms have their own advantages and disadvantages, the prior art proposes an improved ant colony genetic fusion algorithm (IACO-GA) that integrates GA into the ACO algorithm; However, mobile picking robots in orchards will encounter changeable and complex dynamic environments during operation. They must not only identify and avoid newly added static obstacles, but also respond to sudden dynamic obstacles in real time. The path planning application of IACO-GA in static environments is effective, but the path planning for dynamic obstacles is not flexible enough and cannot avoid dynamic obstacles well. For example, if the robot's sensor detects an unknown dynamic obstacle while it is moving, but it still moves along the originally planned path, this may cause the robot to collide with the obstacle or fail to reach the predetermined position smoothly. Therefore, it is necessary to propose an improved ant colony dynamic path planning method (DIACO-GA) that can adapt well to dynamic obstacles for obstacle avoidance. Summary of the invention

[0004] In view of the above-mentioned prior art, the present invention provides an improved ant colony dynamic path planning method and system to solve the technical problems existing in the above-mentioned background technology.

[0005] In order to achieve the above object, the present invention adopts the following technical means: In a first aspect, the present invention provides an improved ant colony dynamic path planning method, the method comprising the following steps: Step S1: Obtain static environmental information of the orchard, create a 20m*20m orchard raster map based on the static environmental information of the orchard, and formulate a global path based on the DIACO-GA algorithm; Step S2: acquiring the position and posture information of the mobile robot and the dynamic obstacle information, determining the running track of the mobile robot according to the position and posture information of the mobile robot, fusing the position and posture information of the mobile robot with the dynamic obstacle information, and determining the running track of the dynamic obstacle; Step S3: judging whether a collision occurs according to the running track and the moving speed of the dynamic obstacle and the mobile robot, and judging the corresponding collision type, formulating a dynamic obstacle avoidance strategy according to the collision type, and planning a local dynamic obstacle avoidance path; The dynamic obstacle avoidance strategy includes a waiting obstacle avoidance strategy, an even obstacle avoidance strategy, and an odd obstacle avoidance strategy; the even obstacle avoidance strategy includes a first even obstacle avoidance strategy and a second even obstacle avoidance strategy, and the odd obstacle avoidance strategy includes a first odd obstacle avoidance strategy and a second odd obstacle avoidance strategy; Step S4: Fusing the global path with the local dynamic obstacle avoidance path, thereby correcting the global path in real time.

[0006] Optionally, in step S2, the position information of the mobile robot is obtained by a laser radar to determine the running trajectory of the mobile robot; the dynamic obstacle information is obtained by a binocular vision camera, and the position information of the mobile robot is fused with the dynamic obstacle information to determine the running trajectory of the dynamic obstacle.

[0007] Optionally, the collision type includes a first collision type, a second collision type and a third collision type; In step S3, determining the corresponding collision type specifically includes: If the running trajectory of the dynamic obstacle contacts the running trajectory of the mobile robot and a pre-collision point appears, and the running trajectory of the dynamic obstacle and the running trajectory of the mobile robot are not on the same displacement line, then the collision type belongs to the first collision type; If the running trajectory of the dynamic obstacle and the running trajectory of the mobile robot are on the same displacement line, and the moving directions of the dynamic obstacle and the mobile robot are opposite, then the collision type belongs to the second collision type; If the running trajectory of the dynamic obstacle and the running trajectory of the mobile robot are on the same displacement line, and the dynamic obstacle and the mobile robot have the same moving direction but different speeds, then the collision type belongs to the third collision type.

[0008] Optionally, when the collision type belongs to the first collision type, the waiting obstacle avoidance strategy is executed, and the waiting obstacle avoidance strategy is specifically that the mobile robot waits time, after the dynamic obstacle passes the pre-collision point, the mobile robot continues to move along the global path; When the collision type belongs to the second collision type and the third collision type, it is necessary to judge the multiple relationship between the moving step length of the dynamic obstacle and the moving step length of the mobile robot. If the moving step length of the dynamic obstacle is an even multiple of more than twice the moving step length of the mobile robot, the even obstacle avoidance strategy is executed; if the moving step length of the dynamic obstacle is an odd multiple of more than three times the moving step length of the mobile robot, the odd obstacle avoidance strategy is executed.

[0009] Optionally, the waiting obstacle avoidance strategy The calculation method is:

[0010] In the formula, is the side length of the mobile robot, is the side length of the dynamic obstacle, is the speed of the mobile robot, is the speed of the dynamic obstacle.

[0011] Optionally, when the collision type belongs to the second collision type and the third collision type, a safety distance between the mobile robot and the dynamic obstacle needs to be met before a corresponding obstacle avoidance strategy can be executed; The second collision type satisfies the following safety distance calculation formula:

[0012] The third collision type satisfies the following safety distance calculation formula:

[0013] In the formula, Detecting distances of dynamic obstacles for mobile robots; is the obstacle moving speed; is the moving speed of the mobile robot; The number of grids occupied by the obstacle; Modulo operation for obstacles; It is a floor rounding operation; is the number of grids between the mobile robot and the obstacle; To avoid obstacles.

[0014] Optionally, based on the second collision type, after the safe distance between the mobile robot and the dynamic obstacle is satisfied, if the moving step length of the dynamic obstacle is an even multiple of more than twice the moving step length of the mobile robot, the first even obstacle avoidance strategy is executed, and the specific formula of the first even obstacle avoidance strategy is as follows:

[0015] Based on the second collision type, after the safe distance between the mobile robot and the dynamic obstacle is satisfied, if the moving step length of the dynamic obstacle is an odd multiple of more than three times the moving step length of the mobile robot, the first odd obstacle avoidance strategy is executed. The specific formula of the first odd obstacle avoidance strategy is as follows:

[0016] .

[0017] Optionally, based on the third collision type, after the safe distance between the mobile robot and the dynamic obstacle is satisfied, if the moving step length of the dynamic obstacle is an even multiple of more than twice the moving step length of the mobile robot, the second even obstacle avoidance strategy is executed, and the specific formula of the second even obstacle avoidance strategy is as follows:

[0018] Based on the third collision type, if the moving step length of the dynamic obstacle is an odd multiple of more than three times the moving step length of the mobile robot, the second odd obstacle avoidance strategy is executed. The specific formula of the second odd obstacle avoidance strategy is as follows:

[0019] .

[0020] Optionally, the method further includes: evaluating the improved ant colony dynamic path planning method, wherein the evaluation indicators are the global path length and the local planned path length.

[0021] In a second aspect, the present invention further provides an improved ant colony dynamic path planning system, the system is applied to an improved ant colony dynamic path planning method, the system includes a perception module, a data processing and path planning module, a power and control module, and a mobile robot; The perception module includes a drone, a visual camera and a laser radar. The drone is used to obtain image information of the overall environment of the orchard, including static obstacle information in the orchard, target fruit tree location information, starting position information of the mobile robot and passable area information in the orchard. The visual camera is used to obtain dynamic obstacle information in the orchard, and the laser radar is used to obtain position information of the mobile robot. The data processing and path planning module is used to process the overall environmental image information of the orchard acquired by the drone, create a rasterized map of the orchard and formulate a global path, fuse the dynamic obstacle information acquired by the visual camera and the lidar with the position information of the mobile robot, determine the running trajectory of the dynamic obstacle, determine the running trajectory of the mobile robot according to the position information of the mobile robot, determine whether a collision occurs according to the running trajectory and movement speed of the dynamic obstacle and the mobile robot, and judge the corresponding collision type, so as to formulate a corresponding obstacle avoidance strategy, plan a local dynamic obstacle avoidance path, fuse the global path with the local dynamic obstacle avoidance path, and thus correct the global path in real time; The power and control module is used to receive the global path planned by the data processing and path planning module, so as to drive the movement of the mobile robot; The mobile robot is used to receive instructions from the power and control module, move on the global path, and avoid static obstacles and dynamic obstacles.

[0022] The beneficial effects of the present invention are: The present invention provides an improved ant colony dynamic path planning method, which adopts an improved ant colony genetic fusion algorithm. First, a global path from a starting position to a target point of a mobile robot is formulated by obtaining overall environmental information of an orchard, the position and posture information of the mobile robot is obtained to determine the running trajectory of the mobile robot, the dynamic obstacle information in the orchard is obtained, the position and posture information of the mobile robot is integrated with the dynamic obstacle information to determine the running trajectory of the dynamic obstacle, the possible collision type between the mobile robot and the dynamic obstacle is judged based on the running trajectory of the mobile robot and the running trajectory of the dynamic obstacle, different obstacle avoidance strategies are formulated according to different collision types, thereby obtaining a local dynamic obstacle avoidance path, combining the global path with the local dynamic obstacle avoidance path, correcting the global path in real time, and obtaining an optimal path from a starting position to a target point of the mobile robot; the mobile robot can avoid not only static obstacles but also dynamic obstacles, thereby effectively preventing the mobile robot from colliding with obstacles during movement; and the optimal path obtained by the method takes a short time, has a small number of turns, and is more flexible in path planning.

[0023] An improved ant colony dynamic path planning system provided by the present invention obtains relevant data information in the perception module through the cooperation of a perception module, a data processing and path planning module, a power and control module, and a mobile robot, and sends the data information to the data processing and path planning module for processing to obtain an initial global path, a dynamic obstacle running trajectory, and a mobile robot running trajectory. According to the dynamic obstacle running trajectory and the mobile robot running trajectory, a corresponding obstacle avoidance strategy is formulated, a local dynamic obstacle avoidance path is planned, and the global path is corrected in real time in combination with the initial global path and the local dynamic obstacle avoidance path. The power and control module receives the real-time corrected global path, thereby driving the mobile robot to move and realizing the obstacle avoidance of dynamic obstacles and static obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 It is a schematic diagram of the improved ant colony dynamic path planning method of the present invention; Figure 2 An orchard environment map constructed for the present invention; Figure 3 It is the first collision type of the present invention; Figure 4 It is the second collision type of the present invention; Figure 5 It is the third collision type of the present invention; Figure 6 It is a schematic diagram of data before regularization processing in one embodiment of the present invention; Figure 7 It is a schematic diagram of data after regularization processing in one embodiment of the present invention; Figure 8 A schematic diagram of a waiting obstacle avoidance strategy in an embodiment of the present invention; Fig. 9 Schematic diagram of an even-numbered obstacle avoidance strategy in one embodiment of the present invention; Fig.10 A schematic diagram of an odd-number obstacle avoidance strategy in an embodiment of the present invention; Fig.11 It is a schematic diagram of the comparison results of the global path trajectory of the DIACO-GA experiment in a static environment in the present invention; Fig.12 It is a schematic diagram of the comparison results of the global path trajectory of the IACO-GA experiment in a static environment in the present invention; Fig.13 It is a schematic diagram of the comparison results of the convergence speeds of DIACO-GA and IACO-GA obtained by the present invention in a static environment; Fig.14 It is a schematic diagram of the comparison results of collision type-obstacle avoidance trajectory of DIACO-GA in a dynamic environment experiment of the present invention; Fig.15 It is a schematic diagram of the comparison results of the collision type-obstacle avoidance trajectory of the IACO-GA experiment in a dynamic environment in the present invention; Fig.16 It is a schematic diagram of the obstacle avoidance trajectory comparison results of the present invention for the collision type two odd-number obstacle avoidance strategy of DIACO-GA in a dynamic environment experiment; Fig.17 It is a schematic diagram of the obstacle avoidance trajectory comparison results of the present invention for the collision type two odd-number obstacle avoidance strategy of IACO-GA in a dynamic environment experiment; Fig.18 It is a schematic diagram of the obstacle avoidance trajectory comparison results of the collision type two even-number obstacle avoidance strategy of DIACO-GA in a dynamic environment experiment of the present invention; Fig.19 It is a schematic diagram of the obstacle avoidance trajectory comparison results of the present invention for the collision type two even-number obstacle avoidance strategy of IACO-GA in a dynamic environment experiment; Fig. 20 It is a schematic diagram of the obstacle avoidance trajectory comparison results of the three odd-number obstacle avoidance strategies of collision types in the DIACO-GA experiment under a dynamic environment; Fig.21 It is a schematic diagram of the obstacle avoidance trajectory comparison results of the present invention for the three odd-number obstacle avoidance strategies of the collision type of IACO-GA in the dynamic environment experiment; Fig. 22 It is a schematic diagram of the obstacle avoidance trajectory comparison results of the three even-number obstacle avoidance strategies of collision types in the DIACO-GA experiment under a dynamic environment; Fig.23It is a schematic diagram of the obstacle avoidance trajectory comparison results of the three even-number obstacle avoidance strategies of the collision type of IACO-GA in the dynamic environment experiment of the present invention; Fig.24 It is a structural schematic diagram of the improved ant colony dynamic path planning system of the present invention; The realization of the purpose, functional features and advantages of the present invention will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0025] The technical solution of the present invention is further elaborated in detail below in conjunction with the drawings and specific embodiments of the specification. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by technicians in the technical field of the present invention. The terms used in the specification of the present invention herein are only for the purpose of describing specific embodiments and are not intended to limit the present invention. In the following description, the expression "some embodiments" is related to a subset of all possible embodiments, but it should be understood that "some embodiments" can be the same subset or different subsets of all possible embodiments, and can be combined with each other without conflict.

[0026] In the following description, a large number of specific details are provided to provide a more thorough understanding of the present invention. However, it is apparent to those skilled in the art that the present invention can be implemented without one or more of these details. In other examples, in order to avoid confusion with the present invention, some technical features well known in the art are not described.

[0027] It should be understood that the present invention can be implemented in different forms and should not be construed as being limited to the embodiments presented herein. On the contrary, providing these embodiments will make the disclosure thorough and complete, and will fully convey the scope of the present invention to those skilled in the art. And the purpose of the terms used herein is only to describe specific embodiments and is not intended to be limiting of the present invention.

[0028] In order to fully understand the present invention, a detailed structure will be proposed in the following description to illustrate the technical solution proposed by the present invention. The optional embodiments of the present invention are described in detail as follows, but in addition to these detailed descriptions, the present invention may also have other implementations.

[0029] Reference Figure 1 The embodiment of the present invention proposes an improved ant colony dynamic path planning method, which includes the following steps: Step S1: Obtain the static environment information of the orchard, create a 20m*20m orchard grid map based on the static environment information of the orchard, and formulate a global path based on the DIACO-GA algorithm; The overall static environment information of the orchard is obtained through drone aerial photography technology, including static obstacle information in the orchard, target fruit tree location information, starting position information of the mobile robot and passable area information in the orchard. Based on the above information, the raster method is used to create a raster map of the orchard; specifically, refer to Figure 2 , create Figure 2 In the 20m×20m grid environment shown in the figure, based on the DIACO-GA algorithm, the mobile robot needs to find a global path from the starting point (0.5, 19.5) to the end point (19.5, 0.5) while avoiding static and dynamic obstacles. Figure 2 The black grid represents a static obstacle, the white grid represents a passable grid, the green grid represents a dynamic obstacle, and the arrow represents the direction in which the dynamic obstacle moves; Step S2: obtaining the position and posture information of the mobile robot and the dynamic obstacle information, determining the running trajectory of the mobile robot according to the position and posture information of the mobile robot, fusing the position and posture information of the mobile robot with the dynamic obstacle information, and determining the running trajectory of the dynamic obstacle; Specifically, the mobile robot obtains global path information based on the DIACO-GA algorithm, reads path node information, and moves according to the path node information on the global path. The mobile robot obtains its own position and posture information while moving, thereby determining the running trajectory of the mobile robot; obtains dynamic obstacle information, and in a specific posture of the mobile robot, the mobile robot combines the acquired posture information with the dynamic obstacle information for matching and fusion, optimizes the three-dimensional to two-dimensional projection transformation through iterative optimization and Gauss-Newton method, obtains dynamic obstacle position information, realizes accurate positioning of dynamic obstacles, and then uses regularization processing to obtain the dynamic obstacle position information after fusion to determine the running trajectory of the dynamic obstacle; Reference Figure 6-7 , Figure 6 The original shape of the dynamic obstacle is shown in Figure 7 The shape of the dynamic obstacle after regularization is shown in the figure. The red line indicates the moving direction of the obstacle. The dynamic obstacle feature points are extracted based on the dynamic obstacle information detected by the sensor in the mobile robot. The dynamic obstacle is regularized into an n×n grid based on the selected feature points. The blue dotted line indicates the running trajectory of the dynamic obstacle. The purpose of regularization is to prevent the model from overfitting the training data and make the model have good generalization performance. Step S3: judging whether a collision occurs according to the running trajectory and movement speed of the dynamic obstacle and the mobile robot, and judging the corresponding collision type, formulating a dynamic obstacle avoidance strategy according to the collision type, and planning a local dynamic obstacle avoidance path; The dynamic obstacle avoidance strategies include waiting obstacle avoidance strategy, even obstacle avoidance strategy, and odd obstacle avoidance strategy; the even obstacle avoidance strategy includes the first even obstacle avoidance strategy and the second even obstacle avoidance strategy, and the odd obstacle avoidance strategy includes the first odd obstacle avoidance strategy and the second odd obstacle avoidance strategy; In step S3, during the movement of the mobile robot, if no dynamic obstacle is detected, the mobile robot will travel along the planned global path. If a dynamic obstacle is detected, the collision type will be determined based on the running trajectory of the dynamic obstacle and the running trajectory of the mobile robot, so as to implement the corresponding obstacle avoidance strategy. Step S4: Fusing the global path with the local dynamic obstacle avoidance path, thereby correcting the global path in real time; When the mobile robot encounters a dynamic obstacle during movement, it will implement the corresponding obstacle avoidance strategy according to the specific dynamic obstacle, so as to determine the local dynamic obstacle avoidance path. Therefore, the global path will be corrected in real time to obtain a new global path.

[0030] It can be seen from the above embodiments that a global path from the starting position of the mobile robot to the target fruit tree position is formulated according to the overall static environment information of the orchard, and the running trajectory of the mobile robot and the running trajectory of the dynamic obstacles are determined according to the acquired mobile robot posture information and dynamic obstacle information. Then, it is judged whether a collision occurs and the type of collision according to the running trajectory of the mobile robot and the running trajectory of the dynamic obstacles, so as to execute different dynamic obstacle avoidance strategies for different collision types, and then obtain a local obstacle avoidance path for dynamic obstacles, and obtain a new global path by combining the global path and the local path for real-time update; through the fusion of the global path and the local dynamic obstacle avoidance path, the shortest displacement of the mobile robot from the starting position to the target fruit tree position can be achieved, and static obstacles and dynamic obstacles can be avoided flexibly and accurately, and the mobile robot can also reach the target position more quickly.

[0031] As a further explanation of the above embodiment, in step S2, the position information of the mobile robot is obtained by a laser radar to determine the running trajectory of the mobile robot; the dynamic obstacle information is obtained by a binocular vision camera, and the position information of the mobile robot is matched and fused with the dynamic obstacle information to determine the running trajectory of the dynamic obstacle.

[0032] It should be noted that, compared with Beidou or GPS navigation, laser radar has obvious advantages in accuracy, flexibility and position measurement speed, so that it can accurately determine the position and posture of the robot in the complex environment of the orchard. The robot starts from a specified starting point and combines position estimation and various sensor data for real-time positioning during the movement. In the embodiment of the present invention, Silan RPLIDAR-A3 infrared laser radar is used to obtain the robot's position and posture information; The binocular vision camera can process complex data, obtain more information, has high measurement accuracy, and is highly robust to light conditions. In an embodiment of the present invention, a Realsense D455 binocular vision camera is used to obtain dynamic obstacle information; the binocular vision camera sets a three-dimensional visual coordinate system xyz, and the original image collected by the binocular vision camera automatically enters the three-dimensional visual coordinate system for positioning, and the coordinates of each pixel point of the original image are obtained after capture. If the original image includes dynamic obstacles, the coordinate values ​​of each pixel point of the dynamic obstacles can be captured.

[0033] The original image captured by the binocular vision camera includes pixels containing dynamic obstacles and pixels without dynamic obstacles. If the original image does not contain dynamic obstacles, it is an invalid image and is discarded. Only the original image containing dynamic obstacles is a valid image.

[0034] Reference Figure 3-5 , as a further illustration of the above embodiment, the dynamic obstacle collision type includes a first collision type, a second collision type and a third collision type; In step S3, determining the corresponding collision type specifically includes: If the trajectory of the dynamic obstacle contacts the trajectory of the mobile robot, a pre-collision point will appear, and the trajectory of the dynamic obstacle and the trajectory of the mobile robot are not on the same displacement line, then the collision type belongs to the first collision type. If the running trajectory of the dynamic obstacle and the running trajectory of the mobile robot are on the same displacement line, and the moving directions of the dynamic obstacle and the mobile robot are opposite, then the collision type belongs to the second collision type; If the running trajectory of the dynamic obstacle and the running trajectory of the mobile robot are on the same displacement line, and the dynamic obstacle and the mobile robot have the same moving direction but different speeds, then the collision type belongs to the third collision type.

[0035] As a further explanation of the above embodiment, when the collision type belongs to the first collision type, the waiting obstacle avoidance strategy is executed. The waiting obstacle avoidance strategy is specifically that the mobile robot waits Time, after the dynamic obstacle passes the pre-collision point, the mobile robot continues to move along the global path; Specifically, refer to Figure 8 If the dynamic obstacle belongs to the first collision type, that is, during the movement of the dynamic obstacle, the mobile robot detects that there may be a pre-collision point between its running trajectory and the running trajectory of the dynamic obstacle. At this time, the mobile robot pauses and waits before the pre-collision point. Time, after the dynamic obstacle passes the pre-collision point, it moves along the original global path; It should be noted that, since in the first collision type, dynamic obstacles can be avoided by only waiting, therefore, after the mobile robot executes the waiting obstacle avoidance strategy, the local dynamic obstacle avoidance path obtained is consistent with the initially planned global path; When the collision type belongs to the second collision type and the third collision type, it is necessary to determine the multiple relationship between the moving step length of the dynamic obstacle and the moving step length of the mobile robot. If the moving step length of the dynamic obstacle is an even multiple of more than twice the moving step length of the mobile robot, the even obstacle avoidance strategy is executed; if the moving step length of the dynamic obstacle is an odd multiple of more than three times the moving step length of the mobile robot, the odd obstacle avoidance strategy is executed; The moving step length refers to the distance that the mobile robot or dynamic obstacle moves in the orchard raster map, where the mobile robot and dynamic obstacle move one grid in the orchard raster map, representing a step length of one; Specifically, if the collision type belongs to the second collision type and the third collision type, there will be an overlapping part between the running trajectory of the mobile robot and the running trajectory of the dynamic obstacle. Even if the mobile robot waits, it cannot avoid the collision with the dynamic obstacle. The waiting obstacle avoidance strategy is no longer applicable. At this time, the relationship between the multiples of the moving step lengths of the dynamic obstacle and the mobile robot is first determined, and then the corresponding obstacle avoidance strategy is executed according to the determined relationship between the multiples of the moving step lengths of the two.

[0036] As a further illustration of the above scheme, the waiting obstacle avoidance strategy The calculation method is:

[0037] In the formula, is the side length of the mobile robot, is the side length of the dynamic obstacle, is the speed of the mobile robot, is the speed of the dynamic obstacle.

[0038] As a further illustration of the above scheme, when the collision type belongs to the second collision type and the third collision type, the mobile robot also needs to maintain a safe distance from the dynamic obstacle in order to execute the corresponding obstacle avoidance strategy; For the second collision type, the safe distance calculation formula is as follows:

[0039] For the third collision type, the safe distance calculation formula is as follows:

[0040] In the formula, Detecting distances of dynamic obstacles for mobile robots; is the obstacle moving speed; is the moving speed of the mobile robot; The number of grids occupied by the obstacle; Modulo operation for obstacles; It is a floor rounding operation; is the number of grids between the mobile robot and the obstacle; To avoid obstacles.

[0041] Optionally, based on the second collision type, after the safe distance between the mobile robot and the dynamic obstacle is satisfied, if the moving step length of the dynamic obstacle is an even multiple of more than twice the moving step length of the mobile robot, the number of grids between the mobile robot and the dynamic obstacle is If the following equation is satisfied, and there are non-obstacle grids in the 6 grids in the following formula and there is a path to the end point in these grids, then the first even-number obstacle avoidance strategy is executed; the specific formula is as follows:

[0042] Specifically, when the mobile robot sensor detects dynamic obstacle information and regularizes it, the dynamic obstacle has the same speed as the orchard mobile robot and is in the opposite direction, and the step length is 2 times or an even number of times that of the orchard mobile robot. The mobile robot then executes an even obstacle avoidance strategy and plans a local obstacle avoidance route. Fig. 9 ,The orchard mobile robot first moves one grid 45° to the lower left, then one grid 45° to the lower left, and then one grid 45° to the lower right (moving one grid 45° downward, one grid left, and one grid 45° to the upper right can avoid obstacles, but they are excluded because the obstacle avoidance selection points are not optimal), and the dynamic obstacles are successfully avoided. Combined with the planned global path information, the path is replanned, and the shortest path is selected as the driving path of the mobile machine.

[0043] Based on the second collision type, after the safe distance between the mobile robot and the dynamic obstacle is met, if the moving step length of the dynamic obstacle is an odd multiple of three times the moving step length of the mobile robot, and the following formula is satisfied, there are non-obstacle grids in the four grids, and there are paths to the end point in these grids, and the number of grids between the mobile robot and the dynamic obstacle is If the following equation is satisfied, the first odd obstacle avoidance strategy is executed. The specific formula of the first odd obstacle avoidance strategy is as follows:

[0044] Specifically, refer to Fig.10,When the mobile robot sensor detects the dynamic obstacle information and regularizes it, the dynamic obstacle is the same size and opposite to the orchard mobile robot, and the step length is 3 times or an odd number of times of the orchard mobile robot and meets the safety distance, the mobile robot executes the odd obstacle avoidance strategy and plans a local obstacle avoidance path. First, the orchard mobile robot moves one grid 45° to the upper right, then one grid 45° to the lower right, and then 45° to the lower right (moving one grid to the right is excluded because the obstacle avoidance driving path increases). After successfully avoiding the dynamic obstacle, the path is replanned in combination with the planned global path information, and the shortest path is used as the robot driving path.

[0045] As a further illustration of the above scheme, based on the third collision type, after the safe distance between the mobile robot and the dynamic obstacle is met, if the moving step length of the dynamic obstacle is an even multiple of more than twice the moving step length of the mobile robot, the number of grids between the mobile robot and the dynamic obstacle is If the following equation is satisfied, and there are non-obstacle grids in the 6 grids in the following formula, and there is a path to the end point in these grids, then the second even-numbered obstacle avoidance strategy is executed. The specific formula of the second even-numbered obstacle avoidance strategy is as follows:

[0046] Based on the third collision type, if the moving step length of the dynamic obstacle is an odd multiple of three times the moving step length of the mobile robot, and the four grids in the following formula exist non-obstacle grids, and these grids have paths to the end point, and the number of grids between the mobile robot and the dynamic obstacle is If the following equation is satisfied, the second odd obstacle avoidance strategy is executed. The specific formula of the second odd obstacle avoidance strategy is as follows:

[0047] After the even-numbered obstacle avoidance strategy or the odd-numbered obstacle avoidance strategy is executed and the robot moves according to the local dynamic obstacle avoidance path, the path nodes on the local dynamic obstacle avoidance path are merged into the initial global path to form a new global route, and it is determined whether the path node information in the new global route is equal to the path end point. If so, the robot reaches the end point and the path planning ends, otherwise the path planning continues.

[0048] As a further illustration of the above embodiment, it also includes: evaluating the improved ant colony dynamic path planning method, where the evaluation indicators are the global path length and the local planning path length; The global path length refers to the total length of the path that the mobile robot takes from the starting point to the target point to successfully avoid static obstacles and dynamic obstacles. The calculation formula is as follows:

[0049]

[0050] Where: Indicates the number of grids on the path; Indicates the path grids; express arrive The Euclidean distance of The local planning path length refers to the path length taken to avoid dynamic obstacles. As shown below:

[0051] Where: Represents the global path planning after dynamic obstacle avoidance; Indicates global path planning without dynamic obstacle avoidance.

[0052] The present invention is based on DIACO-GA which is improved by IACO-GA. Therefore, in order to show the advantages of DIACO-GA over IACO-GA, the DIACO-GA algorithm is simulated and the global path length is calculated. , local planning path The three evaluation indicators of , number of turns and IACO-GA algorithm are analyzed and compared. In order to ensure the fairness of the simulation experiment, the simulation experiment maps are all adopted. Figure 1 Rasterized map of the orchard; the computer operating system of the experimental platform is Win10 (64-bit), the processor is Corei5-3.1GHz, the memory is 8GB, and the simulation software is MATLAB R2020a.

[0053] (i) Simulation experiment and comparative analysis under static environment In order to verify the feasibility and effectiveness of the improved ant colony dynamic path algorithm (DIACO-GA) in a 20m×20m static simple environment, the DIACO-GA and IACO-GA algorithms were run 20 times in a 20m×20m static simple environment. The simulation results are shown in Table 1-1: Table 1-1 Simulation results of IACO-GA and DIACO-GA in static environment

[0054] Reference Figure 11-13As shown in Table 1-1, the IACO-GA algorithm falls into a local optimum, has large convergence volatility, and converges slowly, resulting in a long global planning path, many turns, and an unsmooth path. Compared with the IACO-GA algorithm, the DIACO-GA algorithm has a shorter path length of 1.64m, a 13.9% faster convergence speed, 9 fewer turns, and a 59% higher smoothness. Therefore, compared with the IACO-GA algorithm, the DIACO-GA algorithm has a stronger global optimization capability, faster convergence speed, and a smoother path in a static environment.

[0055] (ii) Simulation experiments and comparative analysis under dynamic environment In order to verify the feasibility of the DIACO-GA algorithm in path planning in a dynamic environment, the path length is globally planned. , length of local planning path The performance of the DIACO-GA algorithm and the IACO-GA algorithm are analyzed using three evaluation indicators: the number of turns, the number of turns, and the number of turns. In the dynamic path simulation environment, the red grid represents the robot, the black grid represents the static obstacle, and the other colors represent the dynamic obstacles in different motion states. In order to reduce the complexity of dynamic obstacle avoidance, the dynamic obstacles are assumed to be in uniform motion and have a starting point, an end point, a direction, and a speed. The relevant dynamic obstacle parameters are shown in Table 1-2: Table 1-2 Related dynamic obstacle parameter information table

[0056] (1) Collision type - simulation experiment In order to verify the dynamic obstacle avoidance performance and feasibility of the waiting obstacle avoidance strategy proposed by the DIACO-GA algorithm in a 20m×20m dynamic simple environment. The DIACO-GA and IACO-GA algorithms were run 20 times in a 20m×20m dynamic simple environment, and the optimal static global planning path length was selected. , dynamic obstacle avoidance global planning path length , length of local planning path , the simulation experimental data of the number of turns are shown in Table 1-3.

[0057] Table 1-3 IACO-GA and DIACO-GA collision type 1 experimental results in dynamic environment

[0058] Reference Figure 14-15 (in Fig.14 The path planned by the DIACO-GA algorithm, Fig.15The path planned by the IACO-GA algorithm), as shown in Tables 1-3, both the IACO-GA algorithm and the DIACO-GA algorithm of the present invention can successfully avoid obstacles. The length of the local planned path of the DIACO-GA algorithm is 0.39m, and the number of turns is 5 times, while the length of the local planned path of the IACO-GA algorithm is 1.75m, and the number of turns is 9 times; the global path length of the DIACO-GA algorithm is shortened by 3.06m, the local planned path length is shortened by 1.36m, and the smoothness is improved by 20.6%. Since the DIACO-GA algorithm improves the initial pheromone distribution to avoid deadlock and obtain the optimal global path information, it can wait before the pre-collision point to avoid dynamic obstacles, so the local path length increases less, and it only needs to drive according to the previously planned global path planning after the dynamic obstacle passes; while the IACO-GA algorithm falls into the local optimum, resulting in a local obstacle avoidance path length that is too long. Therefore, the DIACO-GA algorithm has a smaller obstacle avoidance action amplitude when encountering dynamic obstacles than the IACO-GA algorithm, a shorter local obstacle avoidance path, a shorter global path planning, and a smoother path.

[0059] (2) Collision type 2 simulation experiment In order to verify the odd and even obstacle avoidance strategies proposed in the DIACO-GA algorithm, the DIACO-GA and IACO-GA algorithms were run 20 times in a 20m×20m dynamic simple environment to obtain the optimal static global planning path length. , dynamic obstacle avoidance global planning path length , length of local planning path , the simulation experimental data of the number of turns are shown in Table 1-4.

[0060] Table 1-4 IACO-GA and DIACO-GA collision type 2 result data table in dynamic environment

[0061] Reference Figure 16-17 (in Fig.16 This is the first odd obstacle avoidance strategy trajectory planned by the DIACO-GA algorithm. Fig.17 is the obstacle avoidance trajectory diagram planned by the IACO-GA algorithm), Table 1-4 shows that the odd obstacle avoidance strategy proposed by the DIACO-GA algorithm completes the obstacle avoidance action more smoothly when facing dynamic obstacles. After the DIACO-GA algorithm and the IACO-GA algorithm safely avoid dynamic obstacles, the DIACO-GA algorithm shortens the global path length by 3.26m, shortens the local planned path length by 1.31m, and reduces the number of turns by 9 times compared with the IACO-GA algorithm. Therefore, the DIACO-GA algorithm has a shorter planned path and an 86.9% higher path smoothness than the IACO-GA algorithm after adopting the odd obstacle avoidance strategy. Figure 18-19 (in Fig.18 This is the first even obstacle avoidance strategy trajectory planned by the DIACO-GA algorithm. Fig.19 The obstacle avoidance trajectory diagram planned by the IACO-GA algorithm) and the simulation results in Table 1-4 show that the even obstacle avoidance strategy proposed by the DIACO-GA algorithm has a smaller obstacle avoidance action when encountering dynamic obstacles. Compared with the IACO algorithm, the DIACO-GA algorithm shortens the global planning path by 3.21m, shortens the local planning path by 1.3m, and reduces the number of turns by 8 times. Therefore, compared with the IACO-GA algorithm, the DIACO-GA algorithm has a shorter planned path and a 53.7% higher path smoothness after adopting the even obstacle avoidance strategy. Because the DIACO-GA algorithm's odd and even obstacle avoidance strategies plan local paths in time before the pre-collision point, combined with the planned global path information, to increase the minimum local path length to avoid dynamic obstacles; while the IACO-GA algorithm adopts a back-off strategy or a waiting strategy, it may fall into a deadlock when encountering dynamic obstacles. In order to achieve the purpose of avoiding dynamic obstacles, the local path is long and has many turning points.

[0062] Therefore, when the DIACO-GA algorithm encounters a collision type II dynamic obstacle, it can use the odd and even obstacle avoidance strategies to avoid the dynamic obstacle to a smaller extent, and the local planning path is shorter and the global path is smoother.

[0063] (3) Collision type three simulation experiment In order to verify the performance of the odd and even obstacle avoidance strategies in the DIACO-GA algorithm when facing dynamic obstacles of type collision three, the DIACO-GA and IACO-GA algorithms were run 20 times in a 20m×20m dynamic simple environment to obtain the optimal static global planning path length. , dynamic obstacle avoidance global planning path length , length of local planning path , simulation experimental data of number of turns, the simulation experimental data are shown in Table 1-5.

[0064] Table 1-5 IACO-GA and DIACO-GA collision type three experimental results in dynamic environment

[0065] Reference Figure 20-21 (in Fig. 20 The second odd obstacle avoidance strategy trajectory planned by the DIACO-GA algorithm. Fig.21The obstacle avoidance trajectory planned by the IACO-GA algorithm), Table 1-5 shows the simulation experimental data. The DIACO-GA algorithm's odd obstacle avoidance strategy is 2.27m shorter than the IACO-GA algorithm in terms of global optimal path length, 0.57m shorter in local path length, 8 fewer turns, and 51.8% higher in smoothness. Figure 22-23 (in Fig. 22 The second even obstacle avoidance strategy trajectory diagram planned by the DIACO-GA algorithm. Fig.23 The obstacle avoidance trajectory diagram planned by the IACO-GA algorithm), experimental data in Tables 1-5 show that the even obstacle avoidance strategy of the DIACO-GA algorithm shortens the global planning path length by 2.39m, the local planning path length by 0.58m, and the number of turns after obstacle avoidance is reduced by 1, and the smoothness is improved by 5.6%. Since the odd and even obstacle avoidance strategies of the DIACO-GA algorithm complete local path planning before predicting the collision point, collisions at the pre-collision point can be avoided, which can effectively reduce the number of turns and the length of the local planning path; while the IACO-GA algorithm cannot accurately predict the location of the collision point. In order to avoid obstacles, it chooses to plan the driving path away from dynamic obstacles, resulting in a large increase in the local path.

[0066] Therefore, when encountering the collision type three dynamic obstacle, the odd and even obstacle avoidance strategies in the DIACO-GA algorithm of the present invention avoid the dynamic obstacle with a smoother trajectory, and the local planning path is shorter and the global path is smoother.

[0067] From the above three types of dynamic obstacle avoidance experiments, we can see that the improved ant colony dynamic path planning algorithm proposed in this paper processes obstacles of different shapes according to rules, fully considers the position of static obstacles and the motion state of dynamic obstacles, and designs different obstacle avoidance strategies. The dynamic obstacle avoidance local planning path of the DIACO-GA algorithm is shorter and has fewer turning points than that of the IACO-GA algorithm, which verifies the feasibility and effectiveness of the dynamic obstacle avoidance strategy of the DIACO-GA algorithm. At the same time, DIACO-GA can avoid obstacles more flexibly than IACO-GA.

[0068] Reference Fig.24 , the present invention also provides an improved ant colony dynamic path planning system, the system is applied to an improved ant colony dynamic path planning method, the system includes a perception module, a data processing and path planning module, a power and control module, and a mobile robot; The perception module includes a drone, a visual camera and a laser radar. The drone is used to obtain image information of the overall environment of the orchard, including static obstacle information in the orchard, target fruit tree location information, the starting position information of the mobile robot and the passable area information in the orchard. The visual camera is used to obtain dynamic obstacle information in the orchard, and the laser radar is used to obtain the position information of the mobile robot. The data processing and path planning module is used to process the image information of the orchard's overall environment obtained by the drone, create a raster map of the orchard and formulate a global path, fuse the dynamic obstacle information obtained by the visual camera and the lidar with the mobile robot's posture information, determine the dynamic obstacle's running trajectory, determine the mobile robot's running trajectory according to the mobile robot's posture information, determine whether a collision occurs according to the running trajectory and movement speed of the dynamic obstacle and the mobile robot, and determine the corresponding collision type, so as to formulate a corresponding obstacle avoidance strategy, plan a local dynamic obstacle avoidance path, fuse the global path with the local dynamic obstacle avoidance path, and thus correct the global path in real time; The power and control module is used to receive the global path planned by the data processing and path planning module, thereby driving the movement of the mobile robot; The mobile robot is used to receive instructions from the power and control module, move on the global path, and avoid static and dynamic obstacles.

[0069] The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. The protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. An improved ant colony dynamic path planning method, characterized in that: The method comprises the following steps: Step S1: acquiring static environmental information of the orchard, creating a rasterized map of the orchard according to the static environmental information of the orchard, and formulating a global path based on the DIACO-GA algorithm; Step S2: acquiring the position and posture information of the mobile robot and the dynamic obstacle information, determining the running track of the mobile robot according to the position and posture information of the mobile robot, fusing the position and posture information of the mobile robot with the dynamic obstacle information, and determining the running track of the dynamic obstacle; Step S3: judging whether a collision occurs according to the running track and the moving speed of the dynamic obstacle and the mobile robot, and judging the corresponding collision type, formulating a dynamic obstacle avoidance strategy according to the collision type, and planning a local dynamic obstacle avoidance path; The dynamic obstacle avoidance strategy includes a waiting obstacle avoidance strategy, an even obstacle avoidance strategy, and an odd obstacle avoidance strategy; the even obstacle avoidance strategy includes a first even obstacle avoidance strategy and a second even obstacle avoidance strategy, and the odd obstacle avoidance strategy includes a first odd obstacle avoidance strategy and a second odd obstacle avoidance strategy; Step S4: Fusing the global path with the local dynamic obstacle avoidance path, thereby correcting the global path in real time.

2. The improved ant colony dynamic path planning method according to claim 1, characterized in that: In step S2, the position information of the mobile robot is obtained by a laser radar to determine the running trajectory of the mobile robot; the dynamic obstacle information is obtained by a binocular vision camera, and the position information of the mobile robot is fused with the dynamic obstacle information to determine the running trajectory of the dynamic obstacle.

3. The improved ant colony dynamic path planning method according to claim 2, characterized in that: The collision types include a first collision type, a second collision type and a third collision type; In step S3, determining the corresponding collision type specifically includes: If the running trajectory of the dynamic obstacle contacts the running trajectory of the mobile robot and a pre-collision point appears, and the running trajectory of the dynamic obstacle and the running trajectory of the mobile robot are not on the same displacement line, then the collision type belongs to the first collision type; If the running trajectory of the dynamic obstacle and the running trajectory of the mobile robot are on the same displacement line, and the moving directions of the dynamic obstacle and the mobile robot are opposite, then the collision type belongs to the second collision type; If the running trajectory of the dynamic obstacle and the running trajectory of the mobile robot are on the same displacement line, and the dynamic obstacle and the mobile robot have the same moving direction but different speeds, then the collision type belongs to the third collision type.

4. The improved ant colony dynamic path planning method according to claim 3, characterized in that: When the collision type belongs to the first collision type, the waiting obstacle avoidance strategy is executed. The waiting obstacle avoidance strategy is specifically that the mobile robot waits time, after the dynamic obstacle passes the pre-collision point, the mobile robot continues to move along the global path; When the collision type belongs to the second collision type and the third collision type, it is necessary to judge the multiple relationship between the moving step length of the dynamic obstacle and the moving step length of the mobile robot. If the moving step length of the dynamic obstacle is an even multiple of more than twice the moving step length of the mobile robot, the even obstacle avoidance strategy is executed; if the moving step length of the dynamic obstacle is an odd multiple of more than three times the moving step length of the mobile robot, the odd obstacle avoidance strategy is executed.

5. The improved ant colony dynamic path planning method according to claim 4, characterized in that: The waiting obstacle avoidance strategy The calculation method is: In the formula, is the side length of the mobile robot, is the side length of the dynamic obstacle, is the speed of the mobile robot, is the speed of the dynamic obstacle.

6. The improved ant colony dynamic path planning method according to claim 4, characterized in that: When the collision type belongs to the second collision type and the third collision type, the mobile robot also needs to maintain a safe distance from the dynamic obstacle in order to execute the corresponding obstacle avoidance strategy; The second collision type satisfies the following safety distance calculation formula: The third collision type satisfies the following safety distance calculation formula: In the formula, Detecting distances of dynamic obstacles for mobile robots; is the obstacle moving speed; is the moving speed of the mobile robot; The number of grids occupied by the obstacle; Modulo operation for obstacles; It is a floor rounding operation; is the number of grids between the mobile robot and the obstacle; To avoid obstacles.

7. The improved ant colony dynamic path planning method according to claim 6, characterized in that: Based on the second collision type, after the safe distance between the mobile robot and the dynamic obstacle is satisfied, if the moving step length of the dynamic obstacle is an even multiple of more than twice the moving step length of the mobile robot, the first even obstacle avoidance strategy is executed. The specific formula of the first even obstacle avoidance strategy is as follows: Based on the second collision type, after the safe distance between the mobile robot and the dynamic obstacle is satisfied, if the moving step length of the dynamic obstacle is an odd multiple of more than three times the moving step length of the mobile robot, the first odd obstacle avoidance strategy is executed. The specific formula of the first odd obstacle avoidance strategy is as follows: 。 8. The improved ant colony dynamic path planning method according to claim 6, characterized in that: Based on the third collision type, after the safe distance between the mobile robot and the dynamic obstacle is satisfied, if the moving step length of the dynamic obstacle is an even multiple of more than twice the moving step length of the mobile robot, the second even obstacle avoidance strategy is executed. The specific formula of the second even obstacle avoidance strategy is as follows: Based on the third collision type, if the moving step length of the dynamic obstacle is an odd multiple of more than three times the moving step length of the mobile robot, the second odd obstacle avoidance strategy is executed. The specific formula of the second odd obstacle avoidance strategy is as follows: 。 9. The improved ant colony dynamic path planning method according to claim 1, characterized in that: Also includes: The improved ant colony dynamic path planning method is evaluated, and the evaluation indicators are the global path length and the local planned path length.

10. An improved ant colony dynamic path planning system, the system being applied to an improved ant colony dynamic path planning method as claimed in any one of claims 1 to 9, characterized in that: The system includes a perception module, a data processing and path planning module, a power and control module, and a mobile robot; The perception module includes a drone, a visual camera and a laser radar. The drone is used to obtain image information of the overall environment of the orchard, including static obstacle information in the orchard, target fruit tree location information, starting position information of the mobile robot and passable area information in the orchard. The visual camera is used to obtain dynamic obstacle information in the orchard, and the laser radar is used to obtain position information of the mobile robot. The data processing and path planning module is used to process the overall environmental image information of the orchard acquired by the drone, create a rasterized map of the orchard and formulate a global path, fuse the dynamic obstacle information acquired by the visual camera and the lidar with the position information of the mobile robot, determine the running trajectory of the dynamic obstacle, determine the running trajectory of the mobile robot according to the position information of the mobile robot, determine whether a collision occurs according to the running trajectory and movement speed of the dynamic obstacle and the mobile robot, and judge the corresponding collision type, so as to formulate a corresponding obstacle avoidance strategy, plan a local dynamic obstacle avoidance path, fuse the global path with the local dynamic obstacle avoidance path, and thus correct the global path in real time; The power and control module is used to receive the global path planned by the data processing and path planning module, so as to drive the movement of the mobile robot; The mobile robot is used to receive instructions from the power and control module, move on the global path, and avoid static obstacles and dynamic obstacles.

Images