A beef cattle farm robot path planning method and system

Abstract

The application provides a beef cattle farm robot path planning method and system, constructs an adjacency graph according to the site structure; calculates the distance information and local obstacle density of each passable node as environmental attribute parameters; constructs an improved heuristic information function based on the parameters; in the ant colony algorithm iterative search, the state transition probability is calculated by combining the pheromone concentration and the improved heuristic information function to select the next node, forming an effective path set; after each iteration, the pheromone of the global optimal path and the current iteration optimal path is enhanced according to the dynamic weight, and the pheromone of the current iteration worst path is punished; after the iteration is completed, the global optimal path is post-processed and output. The environmental attribute parameters are introduced to improve the heuristic information function, enhance the path guiding ability in the complex environment; the multi-path enhancement and the worst path punishment are introduced in the pheromone update, the pheromone distribution is optimized, and the convergence speed and stability are improved; the path smoothing processing improves the executability.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and path planning technology for mobile robots, and in particular to a path planning method and system for a beef cattle farm robot. Background Technology

[0002] With the development of large-scale and intelligent livestock farming, robots are increasingly being used in scenarios such as livestock shed inspection, spray disinfection, precision feeding, manure cleaning, and material transportation. Unlike regulated industrial environments, beef cattle farms often feature complex pen layouts, dense fences and feed troughs, varying aisle widths, concentrated local obstacles, and disturbances caused by livestock activities. Similar complexities are also common in unstructured environments such as livestock and poultry farming, greenhouses, and warehousing and logistics.

[0003] Among existing path planning methods, the A-star algorithm (A*) is a prime example. While both the deterministic algorithm and Dijkstra's algorithm offer strong determinism, they suffer from insufficient global adaptability due to the large number of search nodes in complex environments. The Rapidly-exploring Random Tree (RRT) algorithm, though possessing random exploration capabilities, generates paths with numerous redundant transitions and poor smoothness. Traditional ant colony algorithms, as a swarm intelligence method, exhibit strong global search capabilities and robustness, but still have significant drawbacks: First, heuristic information functions are often constructed solely based on the distance from nodes to the target point, lacking expression for local obstacle density, directional consistency, and iteration stage requirements, easily leading to blind searches, detours, and redundant transitions in obstacle-dense areas or narrow passages. Second, the uniform strategy for pheromone evaporation and updating makes it difficult to distinguish between globally good paths, stage-good paths, and poor paths, easily getting trapped in local optima and experiencing decreased convergence efficiency in later stages. Furthermore, real-world environments include both static obstacles such as fences, feeding troughs, walls, and pillars, as well as dynamic occupants such as livestock and workers. Existing methods are mostly based on static modeling, lacking the flexibility to extend to dynamic scenarios.

[0004] Therefore, how to provide a path planning scheme that is applicable to both static and complex environments and can be extended to dynamic occupancy update scenarios has become a technical problem that urgently needs to be solved in this field. Summary of the Invention

[0005] In view of this, embodiments of the present invention provide a method and system for robot path planning in beef cattle farms to solve the problems of difficult and inaccurate path planning.

[0006] To achieve the above objectives, the embodiments of the present invention provide the following technical solutions:

[0007] The first aspect of this invention discloses a robot path planning method for beef cattle farms, the method comprising:

[0008] Construct an adjacency graph based on the site structure information of the beef cattle farm;

[0009] Calculate the environmental attribute parameters of each traversable node in the adjacency graph; wherein, the environmental attribute parameters include the distance information from each traversable node to the target node and the local obstacle density of each traversable node;

[0010] An improved heuristic information function is constructed based on the environmental attribute parameters; the improved heuristic information function includes a target distance term based on the distance information, a correction term based on the local obstacle density, and an adaptive iterative weight factor that changes linearly with the number of iterations;

[0011] During the path iterative search in the adjacency graph using the ant colony algorithm, the state transition probability is calculated based on the pheromone concentration on the current path edge and the improved heuristic information function to select the next node, until the path search from the starting node to the target node is completed, resulting in a path set containing valid paths.

[0012] After the current iteration ends, based on the path set and the weights that change dynamically with the number of iterations, the global best path up to the current time and the best path of the current iteration are positively enhanced with pheromones, and the worst path of the current iteration is negatively deducted with pheromones, so as to update the pheromone concentration on each path edge.

[0013] After reaching the maximum number of iterations, the globally optimal path is output, and the globally optimal path is processed to obtain the final path and output.

[0014] Preferably, the step of constructing an adjacency graph based on the site structure information of the beef cattle farm includes:

[0015] A path planning environment model is constructed based on the site structure information of the beef cattle farm; wherein, the site structure information includes at least the boundaries of the cattle pen, the location of the fence, the location of the feed trough, the location of the water trough, the passage area, and the obstacle area;

[0016] The path planning environment model is discretized into a two-dimensional grid map, wherein the cattle pen boundary, fence, feeding trough, water trough and obstacle area in the path planning environment model are discretized into obstacle grids, and the passable area is discretized into free grids.

[0017] Each of the free grids is mapped to a node, and the connection relationship between each adjacent node is constructed to obtain an adjacency graph.

[0018] Preferably, the calculation of environmental attribute parameters for each traversable node in the adjacency graph includes:

[0019] Calculate the distance information from each walkable node in the adjacency graph to the target node;

[0020] The local obstacle density of each passable node is calculated using a simple proportional method;

[0021] The distance information and the local obstacle density are used as environmental attribute parameters for each passable node in the adjacency graph.

[0022] Preferably, the step of constructing the improved heuristic information function based on the environmental attribute parameters includes:

[0023] A target distance term is constructed based on the distance information in the environmental attribute parameters, and a local obstacle density correction term is constructed based on the local obstacle density in the environmental attribute parameters.

[0024] Multiply the target distance term and the local obstacle density correction term to obtain the basic heuristic information value;

[0025] For each currently passable node in the adjacency graph, calculate the direction vector from the currently passable node to the candidate node, and calculate the direction vector from the currently passable node to the target node;

[0026] Construct a direction consistency correction term based on the direction vector from the currently passable node to the candidate node and the direction vector from the currently passable node to the target node;

[0027] Construct an adaptive iterative weight factor that varies linearly with the number of iterations;

[0028] The product of the basic heuristic information value and the direction consistency correction term is exponentially raised to the power of the adaptive iterative weight factor to generate a comprehensive heuristic information value, thereby constructing an improved heuristic information function.

[0029] Preferably, the step of processing the globally optimal path to obtain and output the final path includes:

[0030] The node sequence of the global optimal path is traversed. If the line between two non-adjacent nodes does not cross the obstacle grid, all intermediate nodes between the current two nodes are deleted to obtain the optimized node sequence.

[0031] The optimized node sequence is used as the final path and output.

[0032] The second aspect of this invention discloses a robot path planning system for beef cattle farms, the system comprising:

[0033] The first building unit is used to construct an adjacency graph based on the site structure information of the beef cattle farm.

[0034] The calculation unit is used to calculate the environmental attribute parameters of each passable node in the adjacency graph; wherein, the environmental attribute parameters include the distance information from each passable node to the target node and the local obstacle density of each passable node;

[0035] The second construction unit is used to construct an improved heuristic information function based on the environmental attribute parameters; the improved heuristic information function includes a target distance term based on the distance information, a correction term based on the local obstacle density, and an adaptive iterative weight factor that changes linearly with the number of iterations;

[0036] The path search unit is used to calculate the state transition probability based on the pheromone concentration on the current path edge and the improved heuristic information function during the path iterative search in the adjacency graph using the ant colony algorithm, so as to select the next node until the path search from the starting node to the target node is completed, and a path set containing valid paths is obtained.

[0037] The update unit is used to, after the current iteration ends, based on the path set and the weights that change dynamically with the number of iterations, perform positive pheromone enhancement on the global best path up to the current time and the best path of the current iteration, and negative pheromone deduction on the worst path of the current iteration, so as to update the pheromone concentration on each path edge.

[0038] The output unit is used to output the globally optimal path after reaching the maximum number of iterations, and to process the globally optimal path to obtain the final path and output it.

[0039] Preferably, the first building unit includes:

[0040] The first construction module is used to construct a path planning environment model based on the site structure information of the beef cattle farm; wherein, the site structure information includes at least the cattle pen boundary, fence location, feed trough location, water trough location, passage area, and obstacle area;

[0041] The discretization module is used to discretize the path planning environment model into a two-dimensional grid map, wherein the cattle pen boundary, fence, feeding trough, water trough and obstacle area in the path planning environment model are discretized into obstacle grids, and the passable area is discretized into free grids.

[0042] The second construction module is used to map each of the free grids into nodes and construct the connection relationship between each adjacent node to obtain an adjacency graph.

[0043] Preferably, the computing unit includes:

[0044] The first calculation module is used to calculate the distance information from each passable node in the adjacency graph to the target node;

[0045] The second calculation module is used to calculate the local obstacle density of each passable node using a simple proportional method.

[0046] The determination module is used to use the distance information and the local obstacle density as environmental attribute parameters for each passable node in the adjacency graph.

[0047] Preferably, the second building unit includes:

[0048] The third construction module is used to construct a target distance item based on the distance information in the environmental attribute parameters, and to construct a local obstacle density correction item based on the local obstacle density in the environmental attribute parameters.

[0049] The third calculation module is used to multiply the target distance term and the local obstacle density correction term to obtain the basic heuristic information value;

[0050] The fourth calculation module is used to calculate, for each currently passable node in the adjacency graph, the direction vector from the currently passable node to the candidate node, and the direction vector from the currently passable node to the target node.

[0051] The fourth construction module is used to construct a direction consistency correction term based on the direction vector from the current passable node to the candidate node and the direction vector from the current passable node to the target node;

[0052] The fifth building module is used to construct an adaptive iterative weight factor that changes linearly with the number of iterations;

[0053] The generation module is used to multiply the basic heuristic information value and the direction consistency correction term by exponentiation of the adaptive iterative weight factor to generate a comprehensive heuristic information value, thereby constructing an improved heuristic information function.

[0054] Preferably, the output unit includes:

[0055] The deletion module is used to traverse the node sequence of the global optimal path. If the line between two non-adjacent nodes does not cross the obstacle grid, then all intermediate nodes between the current two nodes are deleted to obtain the optimized node sequence.

[0056] The output module is used to output the optimized node sequence as the final path.

[0057] Based on the above embodiments of the present invention, a path planning method and system for a beef cattle farm robot is provided. The method constructs an adjacency graph based on site structure information; calculates the distance information and local obstacle density of each node as environmental attribute parameters; constructs an improved heuristic information function based on these parameters; in the iterative search of the ant colony algorithm, the state transition probability is calculated by combining pheromone concentration and the improved heuristic information function to select the next node, forming a set of effective paths; after each iteration, pheromone enhancement is performed on the globally optimal path and the currently iteratively optimal path based on dynamic weights, and pheromone penalty is applied to the currently iteratively worst path; after the iteration ends, the globally optimal path is post-processed and output. This invention enhances path guidance capabilities in complex environments by introducing environmental attribute parameters to improve the heuristic information function; it simultaneously introduces multi-path enhancement and worst-path penalty in pheromone updates to optimize pheromone distribution, improve convergence speed and stability; and it improves executability through path smoothing processing. Attached Figure Description

[0058] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0059] Figure 1 A flowchart of a robot path planning method for a beef cattle farm provided in an embodiment of the present invention;

[0060] Figure 2 A two-dimensional grid map constructed based on an actual beef cattle farm scene, provided in an embodiment of the present invention;

[0061] Figure 3 A schematic diagram of the path output in a grid map by the original elite ant colony algorithm provided in this embodiment of the invention;

[0062] Figure 4 A schematic diagram of the path output by the double-improved ant colony algorithm provided in an embodiment of the present invention;

[0063] Figure 5 A schematic diagram illustrating the creation of a three-dimensional simulation model of a beef cattle farm in the ROS2 and Gazebo simulation platforms, as provided in an embodiment of the present invention.

[0064] Figure 6 This is a schematic diagram of the robot's trajectory generated by the original elite ant colony algorithm provided in an embodiment of the present invention;

[0065] Figure 7 This is a schematic diagram of the trajectory generated by the dual improved ant colony algorithm provided in an embodiment of the present invention;

[0066] Figure 8 This is a structural block diagram of a robot path planning system for a beef cattle farm, provided as an embodiment of the present invention. Detailed Implementation

[0067] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0068] In this application, the terms "comprising," "including," or any other variations thereof are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0069] As can be seen from the background technology, existing path planning methods lack global adaptability, RRT path smoothness is poor, traditional ant colony algorithms have a single heuristic information dimension, and the pheromone update strategy is rigid and prone to getting trapped in local optima. Moreover, existing methods are mostly based on static obstacle modeling, which makes it difficult to adapt to complex scenarios with dynamic occupancy.

[0070] Therefore, this invention provides a path planning method and system for a robot in a beef cattle farm. The method constructs an adjacency graph based on site structure information; calculates the distance information and local obstacle density of each passable node as environmental attribute parameters; constructs an improved heuristic information function based on these parameters; in the iterative search using an ant colony algorithm, it calculates the state transition probability by combining pheromone concentration and the improved heuristic information function to select the next node, forming a set of effective paths; after each iteration, it enhances the global optimal path and the current iteration's optimal path with dynamic weights using pheromone enhancement, and penalizes the worst path in the current iteration using pheromone; after the iteration ends, it performs post-processing on the global optimal path and outputs it. This invention enhances path guidance capabilities in complex environments by introducing environmental attribute parameters to improve the heuristic information function; it simultaneously introduces multi-path enhancement and worst-path penalty during pheromone updates to optimize pheromone distribution, improve convergence speed and stability; and it improves executability through path smoothing.

[0071] See Figure 1 The diagram shows a flowchart of a path planning method for a beef cattle farm robot provided by an embodiment of the present invention.

[0072] It is understandable that this method is specifically a path planning method for beef cattle farm robots based on a dual improvement of heuristic information functions and pheromone update strategies.

[0073] It should be noted that this method is applicable to various livestock robots in farms, preferably spray disinfection robots, but can also be applied to autonomous mobile equipment such as inspection robots, feeding robots, cleaning robots, and material transport robots. This embodiment uses a beef cattle farm as an example, and the path planning object in this embodiment is a beef cattle farm robot, preferably a spray disinfection robot. The path planning task is to generate an optimal, passable, relatively smooth path suitable for robot execution, given a known starting position and target position, avoiding cattle pens, fences, feed troughs, water troughs, and other obstacle areas. The method includes:

[0074] Step S101: Construct an adjacency graph based on the site structure information of the beef cattle farm.

[0075] In the specific implementation step S101, an adjacency graph for ant colony algorithm path search is constructed based on the site structure information of the beef cattle farm environment.

[0076] Specifically, firstly, a path planning environment model is constructed based on the site structure information of the environment; then, the path planning environment model is discretized into a two-dimensional grid map, where the cattle pen boundary, fence, feeding trough, water trough and obstacle area in the path planning environment model are discretized into obstacle grids, and the passable area is discretized into free grids; each free grid is mapped into a node and the connection relationship between each adjacent node is constructed to obtain an adjacency graph.

[0077] It should be noted that the site structure information includes at least the cattle pen boundaries, fence locations, feed trough locations, water trough locations, passageway areas, and obstacle areas.

[0078] The path planning environment model can be derived from actual cattle farm layout data, simulated cattle farm models, or pre-built scene maps.

[0079] It should be added that the constructed adjacency graph specifically uses an eight-neighborhood approach to establish connections. For any free grid node, its neighboring candidate nodes include the free grid nodes above, below, left, right, and four diagonal directions.

[0080] In this embodiment, the edge weights for grid movement are set as follows: adjacent nodes in the horizontal or vertical direction have an edge weight of 1; adjacent nodes in the diagonal direction have an edge weight of 1. .

[0081] Understandably, this setting can match the actual geometric distance differences of grid movement in different directions. Horizontal and vertical movements correspond to unit movement distances, while diagonal movements have a longer actual geometric distance. This not only improves the rationality of path length calculation, but also provides an accurate quantitative basis for subsequent state transition probability calculation, path cost accumulation, and global optimal path evaluation.

[0082] Compared to the four-neighbor connection method, the eight-neighbor connection can provide more candidate expansion directions, and the searched path is closer to the actual running trajectory of the robot. At the same time, it can reduce redundant turning points in the path and improve the smoothness and executability of the path.

[0083] It should be noted that the path planning environment model is based on static obstacle environment modeling. In other implementations, information such as the occupancy status of cattle, the activity area of ​​personnel, and the temporary blocking area can also be combined to update the local grid status, thereby adapting to the scenario requirements of local dynamic occupancy updates.

[0084] Step S102: Calculate the environmental attribute parameters of each traversable node in the adjacency graph.

[0085] The environmental attribute parameters include the distance information from each passable node to the target node and the local obstacle density of each passable node. These environmental attribute parameters are used to subsequently improve the construction of the heuristic information function.

[0086] In the specific implementation of step S102, for each passable node in the adjacency graph, its environmental attribute parameters are calculated.

[0087] Specifically, the process is as follows (processes A1 to A3):

[0088] Process A1: Calculate the distance information from each walkable node in the adjacency graph to the target node.

[0089] First, we set each passable node in the adjacency graph as a candidate node and the destination node as the target node.

[0090] In implementing process A1, the distance information from each passable node in the adjacency graph to the target node is calculated as shown in formula (1).

[0091] Let the candidate node be The target node is Then candidate nodes To the target node The distance is denoted as In this embodiment, the distance information is calculated using Euclidean distance, and its expression is:

[0092] (1).

[0093] In formula (1), Candidate nodes To the target node The Euclidean distance; Candidate nodes coordinates For the target node The coordinates.

[0094] It is understandable that Euclidean distance is used because the current adjacency graph uses an eight-neighbor connection method, and path search supports the expansion of traversable nodes in the horizontal, vertical, and diagonal directions. Euclidean distance can fit the actual geometric relationship and naturally reflect the proximity between traversable nodes and the target point, which can provide a reasonable basis for the construction of subsequent target distance terms.

[0095] Process A2: Calculate the local obstacle density of each passable node using the simple proportional method.

[0096] It should be noted that, in order to characterize the local environmental complexity of the candidate node's location, this embodiment introduces a local obstacle density parameter. This parameter is calculated using a simple ratio method: with the candidate node as the center, the ratio of the number of obstacle grids to the total number of grids in the preset neighborhood is counted, and this ratio is used as the local obstacle density of the corresponding candidate node. Specifically, as shown in formula (2).

[0097] Let the candidate node be Then its local barrier density Defined as:

[0098] (2).

[0099] In formula (2), Candidate nodes Local barrier density; Candidate nodes The number of obstacle grids within the preset neighborhood; Candidate nodes The total number of grid cells within the preset neighborhood range.

[0100] In this embodiment, the preset neighborhood range is preferably a neighborhood range centered on the candidate node with a radius of 2 grid cells. In some other embodiments, the neighborhood range can also be adjusted according to the grid map resolution, obstacle density, and path planning accuracy requirements. The above settings can better balance the local environment representation capability and computational efficiency.

[0101] Understandably, the introduction of local obstacle density means that the evaluation of candidate nodes no longer depends solely on their distance from the target node, but also reflects the distribution of obstacles in the surrounding area: if the area where a candidate node is located has relatively dense obstacles, its local obstacle density is higher, indicating that the passage complexity of the area is higher; if the area where a candidate node is located is relatively open, its local obstacle density is lower, indicating that the area is more conducive to the smooth progress of path search.

[0102] Process A3: Use distance information and local obstacle density as environmental attribute parameters for each passable node in the adjacency graph.

[0103] Understandably, the distance from the candidate node to the target node... Local obstacle density of candidate nodes Together, these constitute the fundamental environmental properties for improving the heuristic information function. Among them, distance information reflects the proximity between the candidate node and the target node, while local obstacle density reflects the complexity of the region where the candidate node is located.

[0104] It should be added that by simultaneously introducing distance information and local obstacle density, the subsequent heuristic information function can take into account both the two key factors of being close to the target and avoiding densely populated areas of local obstacles when evaluating candidate nodes, thereby significantly improving the algorithm's path guidance and obstacle avoidance capabilities in complex aquaculture environments.

[0105] Step S103: Construct an improved heuristic information function based on environmental attribute parameters.

[0106] Understandably, after calculating the environmental attribute parameters, the heuristic information function of the original elite ant colony algorithm is improved to enhance the algorithm's path guidance ability in the complex environment of beef cattle farms.

[0107] The heuristic information function design logic of the original elite ant colony algorithm is relatively simple. The core idea is: the closer the candidate node is to the target node, the higher the corresponding heuristic value. For this path planning problem, the basic heuristic information of the original elite ant colony algorithm can be expressed as:

[0108] (3).

[0109] In formula (3), Candidate nodes in the original elite ant colony algorithm The basic heuristic information value; Candidate nodes To the target node The distance.

[0110] It is understandable that the above formula (3) is a classic heuristic expression in the framework of the original ant colony algorithm and the original elite ant colony algorithm. Its advantage is that it can fully reflect the guiding role of the target node on the candidate node. However, this expression also has obvious shortcomings: it evaluates the candidate node only from the dimension of target distance, without considering the local obstacle distribution in the area where the candidate node is located, and without considering the consistency between the current candidate direction and the target direction. Therefore, in the scenario of complex obstacle distribution and narrow local passages in beef cattle farms, it is easy to cause problems such as blind search, local detours, and redundant turns.

[0111] To enhance the path guidance capability of the original elite ant colony algorithm in the complex environment of beef cattle farms, this embodiment makes targeted improvements to its heuristic information function. Specifically, it introduces three types of optimization terms: local obstacle density correction term, direction consistency correction term, and adaptive iterative weight factor. The improved evaluation of candidate nodes can comprehensively consider the target attraction effect, the complexity of the local environment, and the consistency between the candidate direction and the target direction, while adapting to the search needs of different iteration stages.

[0112] In the specific implementation step S103, an improved heuristic information function is constructed by introducing a local obstacle density correction term, a direction consistency correction term, and an adaptive iterative weight factor based on environmental attribute parameters.

[0113] The specific process is as follows (processes B1 to B6):

[0114] Process B1: Construct the target distance term based on the distance information in the environmental attribute parameters, and construct the local obstacle density correction term based on the local obstacle density in the environmental attribute parameters.

[0115] Process B2: Multiply the target distance term and the local obstacle density correction term to obtain the basic heuristic information value.

[0116] During the implementation of process B2, candidate nodes In the The basic heuristic information value at the next iteration is defined as:

[0117] (4).

[0118] In formula (4), For the first Candidate nodes in the next iteration The basic heuristic information value; This is a scaling factor used to adjust the overall magnitude of the basic heuristic information value; Candidate nodes To the target node The distance; Candidate nodes Local barrier density; This is the obstacle density influence coefficient, used to adjust the degree of attenuation of heuristic information values ​​by local obstacle density.

[0119] in, It inherits the basic idea from the original elite ant colony algorithm that "the closer a candidate node is to the target node, the higher its guiding value".

[0120] Exponential decay term This is used to introduce local obstacle distribution information, so that the heuristic value of the candidate node can decrease as the local obstacle density increases, thereby avoiding the algorithm from blindly expanding in areas with dense obstacles.

[0121] In this embodiment, the preferred values ​​for the parameters are: ; .

[0122] Understandable, Used to adjust the overall scale of the basic heuristic information values; Used to adjust the strength of the influence of local obstacle density on candidate node selection.

[0123] In some specific implementations, the parameters can also be adjusted according to the density of obstacles in the beef cattle farm, the resolution of the raster map, and the accuracy requirements of path planning. Preferably, It can be 0< Values ​​within the range of ≤1 It can take values ​​in the range of 1.0 to 3.0.

[0124] Process B3: For each currently passable node in the adjacency graph, calculate the direction vector from the currently passable node to the candidate node, and calculate the direction vector from the currently passable node to the target node.

[0125] In implementing process B3, to reduce invalid deflections and local detours during path search, for each node in the adjacency graph, the direction vector from the currently traversable node to the candidate node, and the direction vector from the currently traversable node to the target node are calculated, as follows:

[0126] Let the currently passable nodes be The target node is Candidate nodes are Then the direction vector from the currently passable node to the target node is The direction vector from the currently passable node to the candidate node is: .

[0127] The angle between two vectors is denoted as Then the cosine values ​​can be expressed as:

[0128] (5).

[0129] In formula (5), Represents the dot product of two vectors; For vectors The modulus length;

[0130] For vectors The length of the module.

[0131] Process B4: Construct a direction consistency correction term based on the direction vector from the current passable node to the candidate node and the direction vector from the current passable node to the target node.

[0132] In implementing process B4, the direction consistency correction term Defined as:

[0133] (6).

[0134] In formula (6), Candidate nodes Directional consistency correction term.

[0135] It should be noted that when the candidate direction is closer to the target direction, Approaching 1, Approaching 1; when the candidate direction deviates significantly from the target direction, It gets smaller.

[0136] Understandably, the purpose of this directional consistency correction term is to increase the probability of selecting candidate nodes that are consistent with the target direction, thereby reducing unnecessary backtracking and local detours.

[0137] Process B5: Construct an adaptive iterative weight factor that varies linearly with the number of iterations.

[0138] Understandably, in order to balance the exploratory ability in the early stages of the search and the convergence ability in the later stages, an adaptive iterative weight factor is further introduced. Its expression is:

[0139] (7).

[0140] In formula (7), For the first Adaptive iterative weight factor in the next iteration; Minimum weight; The maximum weight; This represents the current iteration number; This represents the maximum number of iterations.

[0141] Formula (7) enables the algorithm to maintain strong exploration diversity in the early stages of the search; as the number of iterations increases, the algorithm's ability to utilize excellent path information is gradually enhanced.

[0142] It should be noted that the preferred values ​​for the parameters are: ; .

[0143] Preferably, A value of 0.1~0.5 is acceptable. A value of 0.8 to 1.5 is acceptable.

[0144] Process B6: Multiply the basic heuristic information value by the direction consistency correction term, and exponentiate it with the adaptive iterative weight factor to generate a comprehensive heuristic information value, thereby constructing an improved heuristic information function.

[0145] In implementing process B6, after comprehensively considering the basic heuristic information value, the direction consistency correction term, and the adaptive iterative weight factor, the candidate nodes... The comprehensive heuristic information value is defined as:

[0146] (8).

[0147] In formula (8), Candidate nodes The comprehensive heuristic information value; Based on heuristic information values; This is a direction consistency correction term; This is an adaptive iterative weighting factor.

[0148] Understandably, the comprehensive heuristic information value is the final expression of the original elite ant colony algorithm after improvements at the heuristic level. Through this comprehensive heuristic information value, the evaluation of candidate nodes no longer relies solely on their distance from the target node, but can simultaneously consider the distribution of local obstacles, the consistency between the candidate direction and the target direction, and the characteristics of the search phase, thereby improving the quality of candidate node selection during the path search process.

[0149] It should be noted that the improved heuristic information function has several advantages over the single distance heuristic of the original elite ant colony algorithm: it can maintain a strong target attraction effect when candidate nodes are close to the target node, it can suppress the blind priority selection of candidate nodes in areas with dense local obstacles, it can increase the probability of candidate nodes in the same direction as the target being selected, and it can dynamically balance search diversity and convergence according to different iteration stages. Therefore, the improved heuristic information function is more suitable for path planning scenarios in beef cattle farms with complex obstacle distribution, limited passage, and high requirements for path feasibility.

[0150] Step S104: During the path iterative search in the adjacency graph using the ant colony algorithm, the state transition probability is calculated based on the pheromone concentration on the current path edge and the improved heuristic information function to select the next node, until the path search from the starting node to the target node is completed, and a path set containing valid paths is obtained.

[0151] Understandably, after completing the construction of the improved heuristic information function, the ant colony algorithm will gradually filter candidate nodes according to the state transition probability to complete the path search from the starting node to the target node.

[0152] It should be noted that the ant is located at the current node. At that time, its set of candidate nodes It consists of the following nodes: and the current node Nodes that are connected in the adjacency graph; nodes whose corresponding grid is a passable area; nodes that have not been visited by the current ant.

[0153] Therefore, the candidate node set Essentially, it is the set of all reachable and unvisited adjacent nodes of the current node.

[0154] It should be noted that the basic form of the state transition probability in the original ant colony algorithm and the original elite ant colony algorithm is as follows: at the current node, the ant selects the next node according to probability rules based on the pheromone concentration on the edges and the heuristic information value of the candidate nodes. For path planning problems, the state transition probability can be expressed as:

[0155] (9).

[0156] In formula (9), For the first In this iteration, the ant starts from the current node. Transfer to candidate node The probability of; For the first During the next iteration, the edge → The concentration of pheromones on the surface; Candidate nodes The comprehensive heuristic information value.

[0157] In formula (9), This is a pheromone importance factor, used to characterize the influence of pheromone concentration on state transition probability; This is a heuristic information importance factor, used to characterize the degree of influence of heuristic information on state transition probabilities; For the current node The set of optional candidate nodes; express Any candidate node in; For the k-th iteration, the edge → The concentration of pheromones on the surface; Candidate nodes The comprehensive heuristic information value.

[0158] It is understandable that the basic structure of the above state transition probability formula follows the classic expression of the original ant colony algorithm and the original elite ant colony algorithm. Its core logic is: the higher the pheromone concentration on the edge and the greater the heuristic information value of the candidate node, the higher the probability that the corresponding candidate node will be selected.

[0159] In this embodiment, the basic heuristic information in the original elite ant colony algorithm is replaced with the comprehensive heuristic information defined in Embodiment 4. This allows the state transition probability to simultaneously reflect the target attraction, the distribution of local obstacles, the consistency between the candidate direction and the target direction, and the search requirements at different iteration stages.

[0160] It's worth noting that to prevent ants from repeatedly visiting the same node and creating loops, a tabu list is set up for each ant to record the nodes it has already visited. When an ant moves from the current node to the next node, that next node is added to the tabu list. During subsequent path searching, nodes already in the tabu list will not be included in the corresponding ant's candidate node set. By introducing the tabu list mechanism, the effectiveness of path searching can be improved, avoiding meaningless loops.

[0161] In this embodiment, the search process of a single ant terminates when any of the following conditions are met: if the ant successfully reaches the target node E, the complete path and corresponding path length of the ant are recorded; if there are no available candidate nodes at the current node, i.e. If the set is empty, the current ant's search process is terminated, and it is considered a failure in this round of search. After all ants have completed a round of search, they will record the paths that successfully reach the target node, and use these as the set of valid paths for subsequent pheromone updates.

[0162] It should be further explained that the basic parameters involved in the state transition probability calculation and path search process include: : Number of ants; Maximum number of iterations; : Pheromone importance factor; Heuristic information importance factor; Initial pheromone concentration.

[0163] in, and These are the basic control parameters in the original ant colony algorithm / original elite ant colony algorithm; and Derived from the state transition probability formula, it is a basic parameter of the original ant colony algorithm / original elite ant colony algorithm; Used to initialize the pheromone concentration on each edge.

[0164] In this embodiment, the preferred parameter values ​​are: ; ; ; ; .

[0165] in, This means that 50 ants are used in each iteration for pathfinding; This indicates that the maximum number of iterations is 100; This indicates that pheromone concentration has a fundamental impact on state transition probability; This indicates that the comprehensive heuristic information has a high weight in path selection, highlighting the guiding role of the improved heuristic information function in this invention; This indicates that the initial pheromone concentration of each edge in the adjacency graph is uniformly set to 8.

[0166] In another embodiment, the above parameters can also be adjusted appropriately according to the map size, obstacle distribution density, and path planning accuracy requirements.

[0167] It should be noted that by synthesizing heuristic information values By introducing the state transition probability formula, the ant's judgment dimensions when selecting candidate nodes are expanded. It no longer only considers the target distance and the pheromone concentration on the edge, but also simultaneously incorporates three types of regulatory factors: the distribution of local obstacles, the consistency between the candidate direction and the target direction, and the characteristics of the current iteration stage. Therefore, compared with the original elite ant colony algorithm, the path search process of this method can adapt to the actual scenario of complex obstacle distribution and limited passage space in beef cattle farms. It makes more reasonable decisions that fit the environmental constraints at each node selection step, which not only reduces unnecessary local detours and redundant path turns, but also effectively improves the search quality of the final path. At the same time, it provides a better path foundation for the subsequent pheromone double improvement update mechanism, further ensuring the guidance of subsequent pheromone updates.

[0168] Step S105: After the current iteration ends, based on the path set and the weights that change dynamically with the number of iterations, positively enhance the pheromone of the global best path up to the current time and the best path of the current iteration, and negatively deduct the pheromone of the worst path of the current iteration, so as to update the pheromone concentration on each path edge.

[0169] It should be noted that the pheromone update in the original ant colony algorithm and the original elite ant colony algorithm usually includes two parts: evaporation and increment. Its basic form can be expressed as:

[0170] (10).

[0171] In formula (10), For the first During the next iteration, the edge → The concentration of pheromones on the surface; For the first After the second iteration → The concentration of pheromones on the surface; The pheromone evaporation coefficient; For the edges in this iteration → The increase in pheromones.

[0172] in, It represents the process of historical pheromone decaying over time and is the basic structure for pheromone updates in the original ant colony algorithm and the original elite ant colony algorithm.

[0173] It is evident that the pheromone update of the original elite ant colony algorithm only positively reinforces the better paths, and has problems such as insufficient differentiation between global and iterative optimal paths, lack of inferior path suppression mechanism, and easy pheromone distribution leading to local optima.

[0174] In this embodiment, pheromone updates consider not only positive reinforcement of good paths but also negative penalties for poor paths. The improved pheromone update formula is:

[0175] (11).

[0176] In formula (11), For the first After the second iteration → The concentration of pheromones on the surface; For the first During the next iteration, the edge → The concentration of pheromones on the surface; The pheromone evaporation coefficient; This is the lower bound of pheromones.

[0177] In formula (11), Edges of the globally optimal path → The increase in pheromones produced; The edge of the optimal path in the current iteration → The increase in pheromones produced; The edge of the worst path in the current iteration → The amount of pheromone deduction generated; Increase the weights for the globally optimal path; Increase the weights of the optimal path in the current iteration; This is the penalty weight for the worst path in the current iteration.

[0178] in, The pheromone evaporation mechanism is derived from the original ant colony algorithm and the original elite ant colony algorithm; This is a core update item; This is a lower bound constraint for pheromones, used to prevent pheromones from being excessively weakened during the punishment process.

[0179] It should be noted that the global optimal path pheromone increment is shown in formula (12).

[0180] (12).

[0181] In formula (12), Edges of the globally optimal path → The increase in pheromones produced; It is a pheromone constant; The globally optimal path length; This is the set of edges contained in the globally optimal path.

[0182] Understandably, this item is used to enhance the optimal path information up to the current iteration, so that the globally optimal path continues to play a guiding role in subsequent searches.

[0183] It should be noted that the pheromone increment of the current iterative optimal path is shown in formula (13).

[0184] (13).

[0185] In formula (13), The edge of the optimal path in the current iteration → The increase in pheromones produced; This represents the optimal path length for the current iteration. This is the set of edges contained in the optimal path of the current iteration.

[0186] Understandably, this item is used to retain information on new paths that perform better in the current iteration, so that the algorithm can still maintain a certain ability to discover new and better paths during the search process.

[0187] It should be noted that the pheromone reduction of the worst path in the current iteration is shown in formula (14).

[0188] (14).

[0189] In formula (14), The edge of the worst path in the current iteration → The corresponding pheromone reduction amount; This is the worst path length in the current iteration; This is the set of edges contained in the worst path of the current iteration.

[0190] It is understandable that the worst path in the current iteration is specifically the path with the longest path length among all valid paths that successfully reach the target node in the current iteration.

[0191] Limiting the worst path in the current iteration to the set of valid paths helps ensure comparability between the worst and best paths and avoids directly including failed paths in the penalty process, which would affect the stability of pheromone updates.

[0192] In practical applications, to ensure that the globally optimal path, the current iteration's optimal path, and the current iteration's worst path play different roles in different search stages, an adaptive weight parameter is further introduced, whose expression is:

[0193] (15).

[0194] (16).

[0195] (17).

[0196] In formulas (15) to (17), This represents the current iteration number; This represents the maximum number of iterations. , These are the initial and final values ​​of the global optimal path enhancement weights, respectively. , These are the initial and final values ​​of the enhancement weights for the optimal path in the current iteration, respectively. , These are the initial and final values ​​of the worst path penalty weight for the current iteration, respectively.

[0197] It is understandable that the above three sets of adaptive weight parameters serve the following purposes:

[0198] It increases with the number of iterations, and is used to enhance the continued guiding role of the global optimal path in the later stages of the search.

[0199] It decreases as the number of iterations increases, which is used to make the new optimal path in the current iteration easier to strengthen in the early stages of the search, while avoiding excessive disturbance to global convergence in the later stages.

[0200] It increases with the number of iterations and is used to more effectively suppress the interference of inferior paths on pheromone distribution in the later stages of the search.

[0201] It should be added that the following parameters are derived from the pheromone update framework of the original ant colony algorithm and the original elite ant colony algorithm: The pheromone evaporation coefficient; This is a pheromone constant.

[0202] In this embodiment, the preferred value is: ; .

[0203] in, Used to control the degree to which historical pheromones are retained; Used to control the magnitude of pheromone increase or decrease corresponding to a unit path length.

[0204] Furthermore, the following parameters are introduced by the dual-improved pheromone update strategy of this invention: This is the lower bound of pheromones; , The initial and final values ​​of the weights for enhancing the globally optimal path; , The initial and final values ​​of the weights for enhancing the optimal path in the current iteration; , The initial and final values ​​are the penalty weights for the worst path in the current iteration.

[0205] In this embodiment, the preferred value is: ; ; .

[0206] in, This is used to limit the lower bound of pheromone concentration, so as to avoid excessive punishment that would cause some edges to lose the possibility of being searched for a long time. , Used to adjust the reinforcement strength of the globally optimal path at different iteration stages; , Used to adjust the reinforcement intensity of the current iteration's optimal path at different iteration stages; , Used to adjust the penalty intensity of the worst path in the current iteration at different iteration stages.

[0207] In some other implementations, the above parameters may also be adjusted appropriately according to the map size, obstacle density, path search complexity, and robot navigation accuracy requirements.

[0208] It is understandable that by improving the pheromone evaporation and update strategies, the guiding role of the globally optimal path can be continuously strengthened; the information of newly discovered good paths in the current iteration can be retained; and the inferior paths in the current iteration can be effectively suppressed, reducing their interference with subsequent searches. Finally, the strength of the reinforcement of good paths and the punishment of inferior paths can be dynamically balanced according to different search stages. Therefore, the pheromone dual improvement update strategy in this embodiment has stronger adaptability and is more suitable for path planning scenarios in beef cattle farms where obstacles are complex and the search process is prone to getting stuck in detours and local optima.

[0209] Step S106: After reaching the maximum number of iterations, output the globally optimal path, process the globally optimal path to obtain the final path and output it.

[0210] Understandably, in a grid map environment, because the path search results consist of discrete node sequences, even if the globally optimal path is already relatively good in a search sense, there may still be many locally redundant turning points and excessively dense path polylines. These situations will affect the robot's continuous tracking ability and motion stability during actual navigation.

[0211] Therefore, after completing the path search of the double-improved ant colony algorithm and obtaining the globally optimal path, redundant nodes are removed and smoothed in the output path to further improve the smoothness and executability of the path.

[0212] In the specific implementation of step S106, after reaching the maximum number of iterations, the global optimal path is output, and the node sequence of the global optimal path is traversed. If the connection between two non-adjacent nodes does not cross the obstacle grid, all intermediate nodes between the current two nodes are deleted to obtain the optimized node sequence. The optimized node sequence is used as the final path and output.

[0213] It should be noted that the original path node sequence is:

[0214] (18).

[0215] In formula (18), This is the original path node sequence obtained from the path search. The starting node; For the target node; This is an intermediate path node.

[0216] Specifically, if node With nodes (in > If the straight line segment between +1) does not cross the obstacle area, the node can be deleted. to One or more intermediate nodes in the path can be used to shorten the sequence of path nodes and reduce unnecessary turns.

[0217] It should be noted that the path optimization process includes the following procedures (procedures C1 to C4):

[0218] Process C1: Starting from the starting node of the globally optimal path, examine subsequent nodes that are further away in sequence.

[0219] Process C2: Perform a straight-line reachability test between the current node and subsequent candidate nodes.

[0220] Process C3: When the connection between two nodes does not cross the obstacle area, delete the intermediate redundant nodes.

[0221] Process C4: Repeat the above process until the entire path is traversed to obtain the optimized final path.

[0222] Through the path optimization process described above, this embodiment effectively reduces redundant inflection points in the path while maintaining path traversability, shortens the actual travel distance of the robot, and significantly improves the smoothness and executability of the path, thus making it more conducive to the continuous tracking and precise steering control of the robot chassis in the livestock farm. This optimization, as a post-processing step for the globally optimal path, further transforms the better solution obtained in the search phase into a final navigation path that better matches the actual robot motion characteristics, without changing the aforementioned ant colony algorithm search and pheromone update mechanism. Overall, this improves the practicality and reliability of the path planning results in the complex obstacle environment of a beef cattle farm.

[0223] In this embodiment of the invention, by introducing a local obstacle density correction term, a direction consistency correction term, and an adaptive iterative weight factor into the heuristic information function, the path guidance capability of the original elite ant colony algorithm in the complex environment of a beef cattle farm is improved. By simultaneously introducing global optimal path enhancement, current iteration optimal path enhancement, and current iteration worst path penalty during the pheromone update stage, the rationality of pheromone distribution is optimized, the probability of local optimal traps is reduced, and the convergence speed and operational stability of the algorithm are improved. By performing redundant node deletion and smoothing processing on the global optimal path, the smoothness and executability of the path planning results are improved.

[0224] To verify the practical effectiveness of the dual-improved ant colony algorithm proposed in this invention at the level of two-dimensional discrete path planning, a path planning comparison and verification experiment was conducted based on the MATLAB platform: First, a two-dimensional grid map (e.g., a real beef cattle farm scenario) was constructed. Figure 2 As shown in the figure, a corresponding adjacency graph is then built based on the grid map. The original elite ant colony algorithm and the double improved ant colony algorithm are run respectively, and the performance differences between the two in terms of path length, path smoothness and search effect are compared and analyzed.

[0225] It should be noted that this embodiment selects the original elite ant colony algorithm as the comparison baseline, rather than the basic version of the original ant colony algorithm, because the original elite ant colony algorithm itself already includes an additional reinforcement mechanism for the better path, and belongs to the same basic comparison method with stronger performance; on this basis, the embodiment of the present invention further introduces the dual optimization of heuristic information function improvement and pheromone update strategy, which can better highlight the actual improvement effect of the technical solution of the present invention compared with the reinforcement basic ant colony path planning method.

[0226] Combination Figure 3 The diagram shown illustrates the path output of the original elite ant colony algorithm in a grid map. Figure 4 The diagram shows the path output by the dual-improved ant colony algorithm. A comparison reveals that the original elite ant colony algorithm outputs a path with numerous turns and detours on the grid map, exhibiting significant fluctuations in local path direction and a relatively loose overall shape. In contrast, the dual-improved ant colony algorithm proposed in this embodiment outputs a path that more closely aligns with the target's direction of travel, resulting in smoother obstacle avoidance, fewer turning points, and a more compact path shape. Especially in areas with densely distributed obstacles and complex local passageways, the original elite ant colony algorithm is more prone to local detours and redundant backtracking, while the dual-improved ant colony algorithm proposed in this embodiment can more stably select candidate nodes with higher passage efficiency, significantly reducing ineffective detours and local wandering.

[0227] It is understandable that the effectiveness of the dual-improved ant colony algorithm proposed in this embodiment stems from two aspects: First, the heuristic information function integrates local obstacle density, directional consistency, and adaptive iterative weights, enabling the evaluation of candidate nodes to comprehensively reflect the target distance, local complexity, directional matching degree, and iterative stage requirements, thereby reducing blind selection in obstacle-dense areas and increasing the probability of selecting reasonable directions; Second, the pheromone update strategy simultaneously enhances the globally optimal and iteratively optimal paths, penalizes the worst iterative path, and continuously strengthens excellent paths and suppresses inferior paths through adaptive weight dynamic adjustment, thereby improving pheromone distribution and convergence quality.

[0228] like Figure 3 and Figure 4 As shown, in this embodiment, the optimal path length of the original elite ant colony algorithm is 65.9411, and the path length after removing redundant turning points is 47.5630; the optimal path length of the double improved ant colony algorithm proposed in this embodiment is 45.6985, and the path length after removing redundant turning points is 43.8291.

[0229] Therefore, it can be seen that the double-improved ant colony algorithm proposed in this embodiment not only obtains a shorter optimal path in the path search stage, but also exhibits a better final execution path after path optimization. This demonstrates that the embodiment of this invention outperforms the original elite ant colony algorithm in both path length and path smoothness. Therefore, Figure 3 and Figure 4 This indicates that the dual-improved ant colony algorithm proposed in this embodiment of the invention can achieve better path planning results than the original elite ant colony algorithm in the same two-dimensional grid environment. This is manifested in shorter paths, smoother paths, and fewer redundant turning points, demonstrating that the embodiments of the invention have significant effects on two-dimensional discrete path planning.

[0230] To further verify the application effect of the present invention in a more realistic farm environment, this embodiment establishes a three-dimensional simulation model of a beef cattle farm in Robot Operating System Version 2 (ROS2) and the Gazebo simulation platform (e.g., Figure 5 As shown in the figure, a four-wheel drive simulation vehicle is configured in the simulation model, equipped with one LiDAR and one RGB camera. The original elite ant colony algorithm and the double-improved ant colony algorithm of this invention are deployed in the simulation environment respectively, and the path planning results of the robot from the starting point A to the target point B are compared and verified, and the corresponding path planning trajectory map is output.

[0231] Combination Figure 6 The diagram shows the robot's trajectory generated by the original elite ant colony algorithm and Figure 7 The diagram shows the trajectory generated by the dual-improved ant colony algorithm. In a beef cattle farm simulation model, the robot trajectory generated by the original elite ant colony algorithm has many turns and obvious local detours, resulting in an overall less compact trajectory. In contrast, the trajectory generated by the dual-improved ant colony algorithm of this invention is smoother, has fewer turning points, and the overall path is closer to the target direction. Especially in locally complex areas composed of cattle pens, fences, feeding troughs, and watering troughs, the dual-improved ant colony algorithm of this invention can more stably select the better candidate direction, thereby reducing unnecessary backtracking and local detours.

[0232] from Figure 6 and Figure 7 As can be seen from the comparison, the improved ant colony algorithm of this invention is not only superior in terms of geometric trajectory but also has significant advantages in actual navigation execution. Compared to the original elite ant colony algorithm, the trajectory generated by this invention has more continuous changes and less tortuosity in the main channel and turning areas, which is more conducive to continuous tracking and steering control of the four-wheel drive chassis. In other words, the path output by this invention is not only shorter and has fewer turns, but also makes it easier to achieve smooth navigation in subsequent trajectory tracking, thereby significantly improving the engineering feasibility of the path planning results.

[0233] In another implementation, areas occupied by cattle, human activity, or temporary obstructions can be treated as dynamic obstacles, and the local grid state can be updated during path planning. This update employs a combination of event-triggered and periodic-triggered methods.

[0234] Event triggering conditions include: detecting a cow, person, or temporary obstacle entering the robot's current planned path or its safety buffer zone; detecting that a previously passable grid becomes occupied; or detecting that a previously obstructed grid becomes passable again.

[0235] The periodic triggering method involves updating the local grid area around the robot at preset time intervals. This interval can be determined based on the robot's speed, sensor sampling frequency, and the degree of dynamic interference in the field, and is preferably 0.5 to 2 seconds; in other embodiments, local map updates can also be performed at a frequency of 1 to 5 Hz.

[0236] When a local passageway is temporarily blocked by the movement of a cow, the system updates the relevant grid state and recalculates the local obstacle density and comprehensive heuristic information value of the affected candidate nodes, thereby achieving path adjustment. Because the candidate node evaluation in this invention considers both local obstacle density and directional consistency, and pheromone updates have a suppressive effect on poor paths, this method has better adaptability to local temporary obstruction areas under dynamic occupancy update conditions, effectively improving local avoidance performance.

[0237] It should be noted that this dynamic occupancy update scheme is an extended application based on static obstacle path planning. Its core is still based on the dual improved heuristic information function and dual improved pheromone update strategy proposed in this invention.

[0238] therefore, Figure 6 and Figure 7 This demonstrates that the dual-improved ant colony algorithm of this invention can obtain shorter, smoother, and more suitable planning trajectories for robot execution in a beef cattle farm simulation environment, further verifying the application potential of this invention in actual livestock robot navigation tasks.

[0239] In this embodiment of the invention, based on static obstacle environment modeling, local environmental information can be adjusted by combining the occupancy update of cattle or personnel. Under dynamic occupancy update conditions, the aforementioned improved heuristic information function and pheromone update strategy can enhance the adaptability of path search to temporary obstruction areas and improve local avoidance effects. In addition, this invention is applicable to the path planning tasks of various breeding robots such as spray disinfection robots, inspection robots, feeding robots, cleaning robots and transportation robots in beef cattle farms, and has good versatility.

[0240] Corresponding to the path planning method for a beef cattle farm robot provided in the above embodiments of the present invention, see also... Figure 8The diagram shows a structural block diagram of a robot path planning system for a beef cattle farm provided by an embodiment of the present invention.

[0241] The system includes: a first construction unit 801, a calculation unit 802, a second construction unit 803, a path search unit 804, an update unit 805, and an output unit 806.

[0242] The first building unit 801 is used to build an adjacency graph based on the site structure information of the beef cattle farm.

[0243] The calculation unit 802 is used to calculate the environmental attribute parameters of each passable node in the adjacency graph; wherein, the environmental attribute parameters include the distance information from each passable node to the target node and the local obstacle density of each passable node.

[0244] The second building unit 803 is used to build an improved heuristic information function based on environmental attribute parameters. The improved heuristic information function includes a target distance term based on distance information, a correction term based on local obstacle density, and an adaptive iterative weight factor that changes linearly with the number of iterations.

[0245] The path search unit 804 is used to calculate the state transition probability based on the pheromone concentration on the current path edge and the improved heuristic information function during the path iterative search in the adjacency graph using the ant colony algorithm, so as to select the next node, until the path search from the starting node to the target node is completed, and a path set containing valid paths is obtained.

[0246] Update unit 805 is used to update the pheromone concentration on each path edge after the current iteration ends, based on the path set and the weights that change dynamically with the number of iterations, to positively enhance the global best path up to the current time and the best path in the current iteration, and to negatively deduct the pheromone from the worst path in the current iteration.

[0247] Output unit 806 is used to output the globally optimal path after reaching the maximum number of iterations, and to process the globally optimal path to obtain the final path and output it.

[0248] In this embodiment of the invention, by introducing a local obstacle density correction term, a direction consistency correction term, and an adaptive iterative weight factor into the heuristic information function, the path guidance capability of the original elite ant colony algorithm in the complex environment of a beef cattle farm is improved. By simultaneously introducing global optimal path enhancement, current iteration optimal path enhancement, and current iteration worst path penalty during the pheromone update stage, the rationality of pheromone distribution is optimized, the probability of local optimal traps is reduced, and the convergence speed and operational stability of the algorithm are improved. By performing redundant node deletion and smoothing processing on the global optimal path, the smoothness and executability of the path planning results are improved.

[0249] Combination Figure 8 The content shown, the first building unit, includes: a first building module, a discretization module and a second building module.

[0250] The first construction module is used to build a path planning environment model based on the site structure information of the beef cattle farm; wherein, the site structure information includes at least the boundaries of the cattle pen, the location of the fence, the location of the feed trough, the location of the water trough, the passage area, and the obstacle area.

[0251] The discretization module is used to discretize the path planning environment model into a two-dimensional grid map. The cattle pen boundary, fence, feed trough, water trough and obstacle area in the path planning environment model are discretized into obstacle grids, and the passable area is discretized into free grids.

[0252] The second building module is used to map each free grid into nodes and build the connection relationship between each adjacent node to obtain an adjacency graph.

[0253] Combination Figure 8 The calculation unit shown includes: a first calculation module, a second calculation module, and a determination module.

[0254] The first calculation module is used to calculate the distance information from each walkable node in the adjacency graph to the target node.

[0255] The second calculation module is used to calculate the local obstacle density of each passable node using a simple proportional method.

[0256] The determination module is used to use distance information and local obstacle density as environmental attribute parameters for each passable node in the adjacency graph.

[0257] Combination Figure 8 The content shown, the second building unit, includes: a third building module, a third calculation module, a fourth calculation module, a fourth building module, a fifth building module, and a generation module.

[0258] The third construction module is used to construct the target distance item based on the distance information in the environmental attribute parameters, and to construct the local obstacle density correction item based on the local obstacle density in the environmental attribute parameters.

[0259] The third calculation module is used to multiply the target distance term and the local obstacle density correction term to obtain the basic heuristic information value.

[0260] The fourth calculation module is used to calculate, for each currently passable node in the adjacency graph, the direction vector from the currently passable node to the candidate node, and the direction vector from the currently passable node to the target node.

[0261] The fourth construction module is used to construct a direction consistency correction term based on the direction vector from the current passable node to the candidate node and the direction vector from the current passable node to the target node.

[0262] The fifth building block is used to construct an adaptive iterative weight factor that changes linearly with the number of iterations.

[0263] The generation module is used to multiply the basic heuristic information value with the direction consistency correction term and then exponentially operate with the adaptive iterative weight factor as the exponent to generate a comprehensive heuristic information value, thereby constructing an improved heuristic information function.

[0264] Combination Figure 8 The output unit shown includes a deletion module and an output module.

[0265] The deletion module is used to traverse the node sequence of the globally optimal path. If the line between two non-adjacent nodes does not cross the obstacle grid, then all intermediate nodes between the current two nodes are deleted to obtain the optimized node sequence.

[0266] The output module is used to output the optimized node sequence as the final path.

[0267] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on describing the differences from other embodiments. In particular, for system or system embodiments, since they are basically similar to method embodiments, the description is relatively simple, and relevant parts can be referred to the descriptions in the method embodiments. The systems and system embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without creative effort.

[0268] Those skilled in the art will further recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0269] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A robot path planning method for beef cattle farms, characterized in that, The method includes: Construct an adjacency graph based on the site structure information of the beef cattle farm; Calculate the environmental attribute parameters of each traversable node in the adjacency graph; wherein, the environmental attribute parameters include the distance information from each traversable node to the target node and the local obstacle density of each traversable node; An improved heuristic information function is constructed based on the environmental attribute parameters; the improved heuristic information function includes a target distance term based on the distance information, a correction term based on the local obstacle density, and an adaptive iterative weight factor that changes linearly with the number of iterations; During the path iterative search in the adjacency graph using the ant colony algorithm, the state transition probability is calculated based on the pheromone concentration on the current path edge and the improved heuristic information function to select the next node, until the path search from the starting node to the target node is completed, resulting in a path set containing valid paths. After the current iteration ends, based on the path set and the weights that change dynamically with the number of iterations, the global best path up to the current time and the best path of the current iteration are positively enhanced with pheromones, and the worst path of the current iteration is negatively deducted with pheromones, so as to update the pheromone concentration on each path edge. After reaching the maximum number of iterations, the globally optimal path is output, and the globally optimal path is processed to obtain the final path and output.

2. The method according to claim 1, characterized in that, The construction of the adjacency graph based on the site structure information of the beef cattle farm includes: A path planning environment model is constructed based on the site structure information of the beef cattle farm; wherein, the site structure information includes at least the boundaries of the cattle pen, the location of the fence, the location of the feed trough, the location of the water trough, the passage area, and the obstacle area; The path planning environment model is discretized into a two-dimensional grid map, wherein the cattle pen boundary, fence, feeding trough, water trough and obstacle area in the path planning environment model are discretized into obstacle grids, and the passable area is discretized into free grids. Each of the free grids is mapped to a node, and the connection relationship between each adjacent node is constructed to obtain an adjacency graph.

3. The method according to claim 1, characterized in that, The calculation of environmental attribute parameters for each walkable node in the adjacency graph includes: Calculate the distance information from each walkable node in the adjacency graph to the target node; The local obstacle density of each passable node is calculated using a simple proportional method; The distance information and the local obstacle density are used as environmental attribute parameters for each passable node in the adjacency graph.

4. The method according to claim 1, characterized in that, The step of constructing the improved heuristic information function based on the environmental attribute parameters includes: A target distance term is constructed based on the distance information in the environmental attribute parameters, and a local obstacle density correction term is constructed based on the local obstacle density in the environmental attribute parameters. Multiply the target distance term and the local obstacle density correction term to obtain the basic heuristic information value; For each currently passable node in the adjacency graph, calculate the direction vector from the currently passable node to the candidate node, and calculate the direction vector from the currently passable node to the target node; Construct a direction consistency correction term based on the direction vector from the currently passable node to the candidate node and the direction vector from the currently passable node to the target node; Construct an adaptive iterative weight factor that varies linearly with the number of iterations; The product of the basic heuristic information value and the direction consistency correction term is exponentially raised to the power of the adaptive iterative weight factor to generate a comprehensive heuristic information value, thereby constructing an improved heuristic information function.

5. The method according to claim 1, characterized in that, The process of processing the globally optimal path to obtain and output the final path includes: The node sequence of the global optimal path is traversed. If the line between two non-adjacent nodes does not cross the obstacle grid, all intermediate nodes between the current two nodes are deleted to obtain the optimized node sequence. The optimized node sequence is used as the final path and output.

6. A robot path planning system for a beef cattle farm, characterized in that, The system includes: The first building unit is used to construct an adjacency graph based on the site structure information of the beef cattle farm. The calculation unit is used to calculate the environmental attribute parameters of each passable node in the adjacency graph; wherein, the environmental attribute parameters include the distance information from each passable node to the target node and the local obstacle density of each passable node; The second construction unit is used to construct an improved heuristic information function based on the environmental attribute parameters; the improved heuristic information function includes a target distance term based on the distance information, a correction term based on the local obstacle density, and an adaptive iterative weight factor that changes linearly with the number of iterations; The path search unit is used to calculate the state transition probability based on the pheromone concentration on the current path edge and the improved heuristic information function during the path iterative search in the adjacency graph using the ant colony algorithm, so as to select the next node until the path search from the starting node to the target node is completed, and a path set containing valid paths is obtained. The update unit is used to, after the current iteration ends, based on the path set and the weights that change dynamically with the number of iterations, perform positive pheromone enhancement on the global best path up to the current time and the best path of the current iteration, and negative pheromone deduction on the worst path of the current iteration, so as to update the pheromone concentration on each path edge. The output unit is used to output the globally optimal path after reaching the maximum number of iterations, and to process the globally optimal path to obtain the final path and output it.

7. The system according to claim 6, characterized in that, The first building unit includes: The first construction module is used to construct a path planning environment model based on the site structure information of the beef cattle farm; wherein, the site structure information includes at least the cattle pen boundary, fence location, feed trough location, water trough location, passage area, and obstacle area; The discretization module is used to discretize the path planning environment model into a two-dimensional grid map, wherein the cattle pen boundary, fence, feeding trough, water trough and obstacle area in the path planning environment model are discretized into obstacle grids, and the passable area is discretized into free grids. The second construction module is used to map each of the free grids into nodes and construct the connection relationship between each adjacent node to obtain an adjacency graph.

8. The system according to claim 6, characterized in that, The computing unit includes: The first calculation module is used to calculate the distance information from each passable node in the adjacency graph to the target node; The second calculation module is used to calculate the local obstacle density of each passable node using a simple proportional method. The determination module is used to use the distance information and the local obstacle density as environmental attribute parameters for each passable node in the adjacency graph.

9. The system according to claim 6, characterized in that, The second building unit includes: The third construction module is used to construct a target distance item based on the distance information in the environmental attribute parameters, and to construct a local obstacle density correction item based on the local obstacle density in the environmental attribute parameters. The third calculation module is used to multiply the target distance term and the local obstacle density correction term to obtain the basic heuristic information value; The fourth calculation module is used to calculate, for each currently passable node in the adjacency graph, the direction vector from the currently passable node to the candidate node, and the direction vector from the currently passable node to the target node. The fourth construction module is used to construct a direction consistency correction term based on the direction vector from the current passable node to the candidate node and the direction vector from the current passable node to the target node; The fifth building module is used to construct an adaptive iterative weight factor that changes linearly with the number of iterations; The generation module is used to multiply the basic heuristic information value and the direction consistency correction term by exponentiation of the adaptive iterative weight factor to generate a comprehensive heuristic information value, thereby constructing an improved heuristic information function.

10. The system according to claim 6, characterized in that, The output unit includes: The deletion module is used to traverse the node sequence of the global optimal path. If the line between two non-adjacent nodes does not cross the obstacle grid, then all intermediate nodes between the current two nodes are deleted to obtain the optimized node sequence. The output module is used to output the optimized node sequence as the final path.

Images