A three-dimensional mountain robot path planning method based on ant colony algorithm and geometric optimization

Abstract

The application provides a three-dimensional mountain robot path planning method based on an ant colony algorithm and geometric optimization, and relates to the technical field of three-dimensional mountain robot path planning. A three-dimensional mountain feasible space is constructed based on a three-dimensional mountain terrain height numerical matrix and a height difference passing threshold value, an information matrix is initialized, a potential field heuristic function is constructed, multi-generation ant colony path search is carried out by adopting local information dynamic updating and global information dynamic updating, and a preliminary path is obtained; the preliminary path is subjected to redundant grid deletion and triangular pruning, other grids passed in sequence between two grids directly connected in a theoretical grid path are supplemented, an actual grid path is formed, and a curve is formed for a broken line inflection point. The method can reduce invalid search space, redundant grids and path turns, and reduce the risk of path suspension.

Description

Technical Field

[0001] This invention relates to the field of 3D mountain path planning technology for robots, and in particular to a 3D mountain robot path planning method based on ant colony algorithm and geometry optimization. Background Technology

[0002] With the increasing prevalence of mobile robots used for mountain inspection and exploration, path planning in complex 3D mountain environments has become a hot research topic. Mountainous terrain is characterized by dramatic undulations, cliffs, and abrupt elevation changes, making it difficult for traditional 2D path planning algorithms to adapt to 3D terrain and achieve close-to-the-ground movement. Currently available 3D path planning algorithms include A... Algorithms, genetic algorithms, particle swarm optimization, etc., but A The computational cost of algorithms increases dramatically with increasing dimensionality; genetic algorithms and particle swarm optimization are only quasi-3D planning algorithms. Ant colony optimization has advantages such as distributed computing and swarm intelligence, and has great potential in path planning. It has been successfully applied to 2D path planning and can also be used for 3D path planning. However, existing 3D ant colony optimization schemes still have significant shortcomings: the pheromone mechanism is imperfect, the algorithm is prone to getting trapped in local optima, and it is difficult to plan a globally optimal path; there is a lack of systematic geometric optimization, resulting in many redundant nodes and serious detours in the initial path; and the lack of strict ground-bound constraints makes the path prone to being suspended in the air or passing through mountains, making it unable to cope with complex mountainous terrain.

[0003] The current mainstream 3D modeling method uses the cube global raster method: first, a cube is constructed to plan the space, and then a 3D mesh is generated by equally separating the three axes globally. This method is cumbersome, has a large number of invalid meshes, and significantly increases the overall computational cost of the algorithm.

[0004] Currently, some scholars have proposed multi-algorithm fusion schemes, such as: A Various fusion algorithms have been proposed, including combinations of ant colony optimization (ACO) and particle swarm optimization (PSO) with ant colony optimization, and sparrow search with gray wolf optimization. While these fusion algorithms outperform traditional single algorithms in terms of global search quality and speed, they still suffer from high computational load and difficult code maintenance. Therefore, there is an urgent need for a 3D mountain path planning method that is simple to model, has low computational cost, provides optimal paths, and is adaptable to complex mountainous terrain. Summary of the Invention

[0005] The purpose of this invention is to provide a 3D mountain robot path planning method based on ant colony optimization and geometric optimization. This method can solve the problems of large computational cost of global mesh modeling, easy getting trapped in local optima by ant colony search, many redundant turns in the initial path, and the possibility of the planned path being suspended in the air in existing 3D mountain path planning. It can realize path search, path simplification, and curve processing of polyline inflection points under feasible space constraints.

[0006] To achieve the above objectives, the present invention provides the following solution: A path planning method for a 3D mountain robot based on ant colony optimization and geometry optimization includes: Obtain the three-dimensional mountain terrain height numerical matrix, starting point and ending point. Determine the reachability relationship between adjacent grids based on the comparison results of the mountain height difference and the height difference access threshold. Eliminate inaccessible grids with abrupt height changes to form a three-dimensional mountain feasible space composed of feasible grids and the reachability relationship between adjacent feasible grids. Determine the feasible grids corresponding to the starting point and ending point. The pheromone matrix is ​​initialized for the three-dimensional mountain feasible space, and the potential field heuristic function is constructed based on the three-dimensional straight-line distance from each feasible grid to the corresponding feasible grid at the termination point within the three-dimensional mountain feasible space. Based on the pheromone matrix and potential field heuristic function, a multi-generation ant colony path search is performed from the feasible grid corresponding to the starting point to the feasible grid corresponding to the ending point. Local pheromone dynamic updates are performed during each generation of search, and global pheromone dynamic updates are performed after each generation of search. The search path with the shortest path length in all iterations is determined as the initial path. Redundant meshes are removed from the initial path, and triangulation is performed on the path after removing redundant meshes. When the direct connection between the first and last meshes of a polyline formed by multiple continuous meshes passes through feasible meshes in sequence, and the adjacent meshes passed through by the direct connection in sequence are all reachable, and the actual path length of the direct connection is less than the actual path length of the polyline, the polyline is replaced by the direct connection between the first and last meshes, and triangulation is repeated along the path sequence to form a theoretical mesh path. Complete the other grids that are sequentially passed between two directly connected grids in the theoretical grid path to form the actual grid path composed of adjacent reachable grids in the three-dimensional mountain feasible space; The inflection points of the polyline in the actual grid path are converted into polyline curves to form the robot's walking path.

[0007] The present invention discloses the following beneficial effects: This invention determines the reachability of adjacent grids based on the three-dimensional mountain terrain height matrix and height difference access threshold, and performs path search only within the three-dimensional feasible space of the mountain formed by feasible grids, reducing invalid grids generated by global cube modeling. It constructs a potential field heuristic function using three-dimensional straight-line distances and combines local and global pheromone dynamic updates to reduce the probability of ant colony searches focusing on local paths. Through redundant grid deletion and triangulation pruning, it replaces multiple polylines with direct connections that meet grid reachability conditions and have short lengths, reducing path nodes and unnecessary turns. By completing other grids traversed by direct connections, the actual grid path remains within adjacent reachable grids, and the inflection points of polylines are curved to reduce abrupt turns in the robot's walking path. Attached Figure Description

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

[0009] Figure 1 A flowchart of the method provided in an embodiment of the present invention; Figure 2 A flowchart of path planning execution provided for embodiments of the present invention; Figure 3 The pseudo-color image provided for the embodiments of the present invention; Figure 4 Mesh reachability graph provided for embodiments of the present invention; Figure 5 A preliminary path pseudo-color map provided for embodiments of the present invention; Figure 6 This is a pseudo-color image of the path after deleting redundant grids, provided in an embodiment of the present invention. Figure 7 A pseudo-color image of the mesh path for the triangular pruning theory provided in an embodiment of the present invention; Figure 8 This is a pseudo-color image of the actual mesh path of triangular pruning provided in an embodiment of the present invention; Figure 9 A three-dimensional mountain map provided for an embodiment of the present invention; Figure 10 A three-dimensional pseudo-color map of mountains provided for embodiments of the present invention; Figure 11 This is an example map grid reachability graph provided in an embodiment of the present invention; Figure 12 This is a pseudo-color map of the robot's preliminary path provided in an embodiment of the present invention; Figure 13 This is a pseudo-color image of the robot's path after removing redundant meshes, provided in an embodiment of the present invention. Figure 14 This is a pseudo-color image of the theoretical mesh path for robot triangulation pruning provided in an embodiment of the present invention; Figure 15 A three-dimensional model of the theoretical mesh path for robot triangulation pruning provided in an embodiment of the present invention; Figure 16 This is a pseudo-color image of the actual mesh path for robot triangulation pruning provided in an embodiment of the present invention; Figure 17 A three-dimensional model of the actual mesh path of robot triangulation pruning provided in an embodiment of the present invention; Figure 18 A three-dimensional image of the actual curved path of the robot provided in an embodiment of the present invention; Figure 19 A block diagram of a three-dimensional mountain robot path planning system provided in an embodiment of the present invention. Detailed Implementation

[0010] 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.

[0011] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0012] Figure 1 The method flowchart provided in the embodiments of the present invention is as follows: Figure 1 As shown, this embodiment provides a 3D mountain robot path planning method based on ant colony optimization and geometry optimization, including: Step 100: Obtain the three-dimensional mountain terrain height numerical matrix, starting point and ending point. Determine the reachability relationship between adjacent grids based on the comparison results of the mountain height difference and the height difference access threshold. Eliminate inaccessible grids with abrupt height changes to form a three-dimensional mountain feasible space composed of feasible grids and the reachability relationship between adjacent feasible grids. Determine the feasible grids corresponding to the starting point and ending point. Step 200: Initialize the pheromone matrix for the three-dimensional mountain feasible space, and construct the potential field heuristic function based on the three-dimensional straight-line distance from each feasible grid to the corresponding feasible grid at the termination point within the three-dimensional mountain feasible space; Step 300: Based on the pheromone matrix and potential field heuristic function, perform multi-generation ant colony path search from the feasible grid corresponding to the starting point to the feasible grid corresponding to the ending point. In each generation of search, local pheromone is dynamically updated, and after each generation of search, global pheromone is dynamically updated. The search path with the shortest path length in all iterations is determined as the initial path. Step 400: Redundant meshes are removed from the initial path, and triangulation is performed on the path after removing redundant meshes. When the direct connection between the first and last meshes of a polyline formed by multiple continuous meshes passes through feasible meshes in sequence, and the adjacent meshes passed through by the direct connection in sequence are all reachable, and the actual path length of the direct connection is less than the actual path length of the polyline, the polyline is replaced by the direct connection between the first and last meshes, and triangulation is repeated along the path sequence to form a theoretical mesh path. Step 500: Complete the other grids that are passed sequentially between two directly connected grids in the theoretical grid path to form the actual grid path composed of adjacent reachable grids in the three-dimensional mountain feasible space; Step 600: Convert the inflection points of the polyline in the actual grid path into polyline curves to form the robot's walking path.

[0013] Figure 2 The flowchart for path planning execution provided in the embodiments of the present invention. Figure 2 The complete execution process is shown in the order of data input, validity verification, ant colony iteration, preliminary path extraction, and geometric optimization. After path planning begins, the 3D mountain terrain height matrix, starting point, and ending point are input, and the 3D mountain feasible space is constructed. Then, the validity of the starting and ending points is checked. Path planning ends when any point is invalid; if both points are valid, it enters the ant colony iteration search. If the number of iterations has not reached the set limit, the process sequentially performs mesh selection for a single ant, local pheromone dynamic update, and global pheromone dynamic update after one generation, before entering the next round of search. After the number of iterations reaches the set limit, the preliminary path with the shortest path length is extracted from all iterations, and then redundant mesh deletion, triangulation pruning, theoretical mesh path completion, and polyline curve transformation are performed sequentially to form the robot's walking path. Figure 2 The processing boxes are connected in the direction of the arrows. The judgment boxes correspond to the validity of the starting point and the end point, the judgment of whether a single ant has reached the end point, and the judgment of whether the number of iterations of the ant colony has reached the set limit.

[0014] In step 100, to avoid performing full-domain cube discretization on the space above the mountain and the interior of the mountain, the input three-dimensional mountain terrain data is first preprocessed to obtain a three-dimensional mountain terrain height numerical matrix. The three-dimensional mountain terrain height numerical matrix is ​​represented as follows: The matrix has 10 rows. The number of columns in the matrix is Each element in the matrix corresponds to the mountain height at a grid location. Matrix elements with a definite height value are non-negative; if a matrix location does not have a definite height value, the corresponding grid is designated as an impassable grid, and impassable grids do not participate in subsequent path search.

[0015] Based on the three-dimensional mountain terrain height numerical matrix A pseudo-color map of mountain elevations is created at each grid location. Following a pre-defined color mapping relationship, different mountain elevations are mapped to different colors, allowing the elevation distribution of the 3D mountain terrain to be displayed within the planar grid. Impassable grids without defined elevation values ​​are represented by blank areas different from the effective elevation grids. This is based on the 3D mountain terrain elevation numerical matrix. It can also create three-dimensional surface maps, namely SURF maps. SURF maps use the grid's horizontal and vertical axes and the mountain's altitude as three coordinate dimensions to display the spatial undulations, slope changes, and elevation abrupt changes of the mountain surface. Pseudo-color maps are used to maintain the planar positional relationships of each grid and display height differences, while SURF maps are used to display the three-dimensional morphology of the corresponding mountain surface.

[0016] Figure 3 This is a pseudo-color image provided for an embodiment of the present invention. Figure 3 The grid shown has an abscissa range of 1 to 15 and an ordinate range of 1 to 12. Each colored square corresponds to a valid grid in the three-dimensional mountain terrain height numerical matrix. Figure 3 A height color scale is set on the right, ranging from approximately 5 to 25, with colors from dark to light indicating a gradual increase in mountain altitude; the overall mountain altitude in the map increases from the lower left to the upper right. This map retains the grid row and column positions, the height of each grid cell, and local elevation changes for use in conjunction with... Figure 4 The reachability relationships of the shown mesh and Figures 5 to 8 The corresponding paths for each stage are shown.

[0017] Numerical matrix of three-dimensional mountain terrain height In the adjacent grid, compare the mountain height difference and the height difference access threshold between adjacent grids. In this embodiment, the height difference passage threshold... Set to 3. The difference in mountain elevation between adjacent grid cells should not exceed the elevation difference access threshold. When the mountain height difference between adjacent grids is greater than the height difference access threshold, the corresponding adjacent grids are determined to be reachable. When this occurs, the corresponding adjacent grid is determined to be unreachable. Height difference access threshold. This is used to limit the robot's movement range in adjacent grids, so that subsequent path searches can avoid cliffs, locations with abrupt changes in elevation, and terrain areas that exceed the robot's traversal capabilities.

[0018] Figure 4 The mesh reachability graph provided for embodiments of the present invention. Figure 4 Each point in the corresponding Figure 3 In a grid, when two points are connected by a line, it indicates that the corresponding grids are adjacent and the difference in mountain elevation does not exceed the elevation difference passage threshold. When there is no connection between two adjacent points, it means that the corresponding grids are unreachable. Figure 4 The graph also displays the horizontal, vertical, and diagonal grid connections. In the lower right area of ​​the graph, some adjacent points are not fully connected, indicating that the elevation difference of the mountains at the corresponding locations does not meet the passage requirements. Figure 4 The points in Figure 3 The grid positions in the grid correspond one-to-one, thus forming a grid connection topology for ant colony search.

[0019] Impassable grids without a defined height are removed, while feasible grids with defined heights and the reachability relationships between adjacent feasible grids are retained to form a 3D mountain feasible space. The 3D mountain feasible space is directly constructed based on the height value matrix of the mountain surface, without the need to generate aerial grids or internal mountain grids that are unrelated to the robot's ground-hugging movement.

[0020] Calculate the actual distances between adjacent reachable grids in the 3D mountain terrain. Based on the adjacent reachable grids and their corresponding actual distances, generate an adjacency matrix, a feasible region grid coordinate matrix, and a grid number mapping matrix for each feasible grid. The adjacency matrix records the adjacency and reachability relationships between feasible grids, as well as the actual distances between adjacent reachable grids. The feasible region grid coordinate matrix records the x-coordinate, y-coordinate, and mountain height of each feasible grid. The grid number mapping matrix establishes the correspondence between the grid positions in the 3D mountain terrain height numerical matrix and the grid numbers within the ant colony algorithm. Count the number of feasible grids in the 3D mountain feasible space to determine the size of the feasible region for path planning. .

[0021] Set the starting point for route planning and termination point Starting point and termination point Both the x-coordinate and y-coordinate are integers. For a three-dimensional mountain terrain height numerical matrix... The range of values ​​for the x-axis is limited to... Within, the range of values ​​for the ordinate is limited to... Inside, the vertical coordinates and the three-dimensional mountain terrain height numerical matrix The mountain height remains consistent at the corresponding grid positions.

[0022] Set the number of iterations for the ant colony algorithm. Number of ants in a single round Parameters of pheromone importance Heuristic factor importance parameter pheromone evaporation coefficient and pheromones increase intensity coefficient .

[0023] Before entering the ant colony path search, the starting point and termination point Perform a validity check. Construct a validation matrix. Determine the starting point respectively and termination point Check whether the x-coordinate and y-coordinate exceed the 3D mountain terrain boundary, and determine whether the mountain height of the corresponding grid is a valid value. Starting point or termination point If the point exceeds the 3D mountain terrain boundary, or if the corresponding grid does not have a defined height value, the corresponding point will be identified as an illegal point, an error message will be generated, and path planning will be terminated; starting point and termination point When both pass the validity check, the feasible grid corresponding to the two points is determined and the pheromone and heuristic information initialization is initiated.

[0024] In step 200, the pheromone matrix for the 3D mountain feasible space is initialized for the ant colony algorithm. For adjacent reachable grids... and grid Elements in the pheromone matrix Represents a grid With grid The pheromone concentrations between these concentrations, with the initial pheromone concentration being: Set the minimum pheromone level: minimum pheromone value This is used to limit the lower limit of pheromone concentration, preventing the pheromone concentration from becoming too low after continuous evaporation, which would cause the corresponding grid connection to lose the possibility of being selected.

[0025] Based on the grid number mapping matrix, determine the numbers of the feasible grids corresponding to the starting point and the ending point in the three-dimensional mountain feasible space, and record the feasible grid corresponding to the starting point as the starting node. The feasible grid corresponding to the termination point is recorded as the termination node. In ant colony computation, feasible grids participate in the calculation as nodes. The nodes mentioned below all correspond to feasible grids in the three-dimensional mountain feasible space.

[0026] Termination node Initialize the heuristic information and target node for the objective. Calculate the feasible domain nodes down to the termination node. The three-dimensional straight-line distance is used, and the reciprocal of the three-dimensional straight-line distance is used as the heuristic factor for the corresponding feasible region node. Heuristic factors for feasible domain nodes Calculate according to the following formula: In the formula, Indicates the first The three-dimensional coordinates of each feasible domain node Indicates the termination node The three-dimensional coordinates. Termination node. Since the 3D straight-line distance to itself is zero, the heuristic factor for calculating the termination node position is not used in reciprocal distance calculation. Instead, the heuristic factor for the termination node position is set as follows: The closer a feasible domain node is to the termination node The larger the corresponding heuristic factor, the more likely the ant is to move towards the terminal node. The probability of directional movement increases accordingly. A potential field heuristic function is constructed based on the heuristic factors corresponding to all feasible domain nodes.

[0027] In step 300, start Ants take turns foraging, sending out ants in each round. Each ant performs a pathfinding search. Each ant starts at a node... As the initial position, a tabu list is created to record the nodes that have been visited, in order to prevent ants from repeatedly visiting the same node during a search.

[0028] No. Only ants from the node Transfer to node The probability is calculated using the following formula: In the formula, Indicates the first Only ants in the first From node in round path search Transfer to node The probability of; Represents a node With nodes The pheromone concentration between them; Represents a node The corresponding heuristic factor; Parameters indicating the importance of pheromones; Parameters representing the importance of heuristic factors; Indicates the first The set of next nodes that an ant can access.

[0029] A roulette wheel algorithm is used to select the next moving node based on the node's transition probability. After the current ant moves to the next moving node, the current ant's position and search path are updated, and the next moving node is added to the tabu list. Nodes with higher pheromone concentrations or larger heuristic factors have a higher probability of being selected, thus balancing the utilization of existing good paths with guidance towards the target direction.

[0030] During its journey, the ant determines whether it has entered a dead end. If there is no accessible next node, the ant returns to the previous node in its search path. This adds the node with no accessible next node to the tabu list, and the ant selects its next mover from the returned node. This backtracking process allows the ant to continue exploring other feasible paths after entering a branch of a path that cannot be traversed further. If there is an accessible next node, the ant continues towards the termination node. Move until you reach the terminal node. This completes the path search for a single ant.

[0031] During each generation of pathfinding, local pheromone levels are dynamically updated. Within the same generation, after a preceding ant traverses a search path, the pheromone concentration along that path is reduced, decreasing the probability of subsequent ants choosing the same path and increasing the likelihood of other feasible paths being searched, thus expanding the search range for the same generation of ants. This process of node selection, tabu list updates, dead-end backtracking, and local pheromone updates is repeated until all ants in the current generation have completed their pathfinding. The search path and its length are recorded for each ant in each generation.

[0032] At the end of each generation, a global pheromone dynamic update is performed. The global pheromone dynamic update first evaporates the existing pheromones, then adds the pheromones from the current iteration path. The update formula is: The pheromone increment is: In the formula, Indicates the first The total length of the path traveled by the ant. This indicates an increase in pheromone intensity. (This is relevant to mesh connectivity.) Only the search path passes through the grid connection. Ants are counted in the corresponding connections. The search path does not pass through the grid connection. Ants do not add pheromones to corresponding connections. The smaller the total path length, the greater the pheromone increment for the corresponding path connection. After the global pheromone dynamic update is completed, it will be lower than the minimum pheromone value. The pheromone concentration was corrected to This ensures the stable operation of the pheromone positive feedback mechanism.

[0033] Repeat The ant colony path search iterates through all the ant search paths in all iterations and determines the shortest path as the global optimal initial path.

[0034] Figure 5 This is a preliminary path pseudo-color map provided for an embodiment of the present invention. Figure 5 Continue Figure 3 The 15 columns and 12 rows of schematic grids and height color scales shown, along with the superimposed path lines, represent the initial paths formed by multi-generation ant colony path searches. The paths extend from the lower left region to the upper right region, with several local areas exhibiting backtracking, repeated turns, and short-distance detours between adjacent grids. Each grid traversed by the path belongs to the three-dimensional mountain feasible space, and adjacent path grids satisfy reachability relationships. Figure 5 Used to display the status of paths that have not yet undergone geometry optimization after the ant colony search is completed.

[0035] In step 400, redundant meshes are first removed from the initial path. Assume that the initial path contains continuous path nodes: When node For nodes When the reachable neighbor grid is available, delete the node located at the node. With nodes Continuous path grid between: After deletion, a new continuous path grid is formed: The process involves repeatedly evaluating consecutive path nodes along the initial path sequence until no intermediate path grid can be replaced by a direct connection between the first and last reachable neighbor grids. Specifically, a consecutive path segment consisting of the first grid, the last grid, and one or more intermediate grids in between can be selected along the path sequence. If the first and last grids are adjacent reachable grids, the intermediate grids between them are deleted, and a direct connection is established between the first and last grids. This process does not change the path's starting node, ending node, or necessary turning nodes.

[0036] Figure 6 This is a pseudo-color image of the path after deleting redundant grids, provided in an embodiment of the present invention. Figure 6 The grid coordinate range and height color scale in the middle Figure 5 Similarly, the superimposed path lines represent the path after redundant mesh removal. Figure 5 compared to, Figure 6Local loops, repeated turns, and intermediate grids that can be replaced by direct connections between adjacent reachable grids are removed. The path retains the starting position, ending position, and necessary turning positions, and the number of grids traversed and the number of local turns are reduced accordingly.

[0037] After removing redundant meshes, perform triangulation pruning on the path. Consider multiple consecutive nodes in the path: This forms a multi-segment polyline, and the actual path length of the multi-segment polyline is... Select any three consecutive nodes from the node coordinates in the path as pruning units: Calculate the 3D Euclidean distance of the original polyline segment: Determine the node With nodes The straight lines connecting the points pass through the grid in sequence, and the grids traversed by the straight line connections are then filled in as follows: Calculate the actual path length of the completed straight-line connection: Determine the path sequence Determine if both adjacent grids are reachable, and if grids traversed by a straight line connection belong to the feasible 3D mountain space. If both adjacent grids are reachable, and the following conditions are met: At that time, a direct connection is used. Replace polyline path If any condition is not met, no replacement is performed. Triangulation is repeated along the path sequence, and the theoretical mesh path is formed after traversing all path nodes. Let the actual path length of the theoretical grid path be... The theoretical grid path satisfies: Triangular pruning also considers the straight-line distance between nodes, changes in terrain elevation, and the spatial relationship of three consecutive nodes to determine if there are oblique connections that meet the reachability criteria and have a shorter length. The spatial relationship of the three points is used to assist in judging candidate straight-line connections. The actual replacement is still based on the assumption that the straight-line connection passes through all feasible grids, that all adjacent grids are reachable, and so on. As an execution condition. In areas with undulating terrain, the height changes corresponding to candidate connections that satisfy the height difference travel constraint are relatively stable, which can reduce unnecessary turns in the original path and keep the path within the three-dimensional mountainous feasible space.

[0038] Figure 7 The pseudo-color image of the mesh path of the triangular pruning theory provided in the embodiment of the present invention. Figure 7 The grid coordinate range and height color scale in the middle Figure 5 Similarly, the superimposed line segments represent the theoretical mesh paths formed by triangulation. In the illustrated example, the polylines satisfying the feasibility and length conditions are replaced by direct connections pointing from the lower left region to the upper right region. This line segment represents the theoretically preserved node connections, not the robot traversing intermediate meshes; the specific meshes traversed sequentially by the direct connections are completed in step 500.

[0039] In step 500, to convert the theoretical mesh path into a path that the robot can traverse sequentially along adjacent meshes, the other meshes that the robot passes through in sequence between two connecting meshes in the theoretical mesh path are supplemented. For the theoretical mesh path: Complete the other directly connected grids in sequence to form the actual grid path for triangular pruning: In the formula, Represents a node With nodes The direct connections between them pass through other grids in sequence; Represents a node With nodes The direct connections between them pass through other grids in sequence; Represents a node With nodes The direct connections between them pass through other grids in sequence; Represents a node With nodes The direct connections between the grids are sequentially passed through other grids. Each grid after completion is located within the feasible 3D mountainous space, and any two grids with adjacent paths are reachable. The robot can follow the mountainous terrain by following the order of the completed grids, avoiding path suspension, boundary crossings, or passing through impassable grids caused by theoretically straight connections.

[0040] Figure 8 This is a pseudo-color image of the actual mesh path for triangular pruning provided in an embodiment of the present invention. Figure 8 The grid coordinate range and height color scale in the middle Figure 5 Similarly, the superimposed stepped paths represent the actual mesh paths after completion. Figure 7 The theoretical straight line shown is connected at Figure 8 The process is transformed into a grid sequence consisting of multiple adjacent reachable grids. Each movement occurs between two adjacent reachable grids, and the path always passes through a specific feasible grid and conforms to the discrete mountain surface.

[0041] In step 600, starting from the second grid of the actual grid path, it is determined whether the current grid is a polyline inflection point. If the direction of the path segments before and after the current grid changes, the current grid is identified as a polyline inflection point, and the polyline path at the inflection point is replaced with a curve; if the direction of the path segments before and after the current grid does not change, the corresponding polyline path is retained, and the next grid is checked. The polyline inflection point determination and curve replacement are repeated until the processing of the grid before the termination point is completed, forming the robot's walking path. Polyline curvature is used to reduce abrupt turning of the robot at path inflection points. The curve segments used connect with adjacent path segments at the connection endpoints to form a continuous, differentiable, and smoothly curvatured path; the specific form of the curve can be determined according to the robot's turning radius and motion control constraints, and the curve segments should remain within the passable area.

[0042] To further illustrate the results of 3D mountain feasible space construction, improved ant colony search, and geometric optimization, a 50×50 grid 3D mountain example is used below. The plotting coordinates of the example map are on the x-axis. y-axis The effective grid index is 1 to 50, and the height difference passage threshold is... .

[0043] The impassable grid coordinates are: [5, 2], [5, 3], [5, 5], [5, 11], [5, 27], [5, 30], [5, 43], [5, 44]; [6, 2], [6, 3], [6, 5], [6, 11], [6, 27], [6, 30], [6, 43], [6, 44]; [14, 2], [14, 3 ... 5]、[14,11]、[14,27]、[14,30]、[14,43]、[14,44];[27,2]、[27,3]、[27,5]、[27,11]、[27,27]、[27,30]、[27,43]、[27,44];[32,2]、[32,3]、[32,5]、[32,11]、[32

[27] , [32, 30], [32, 43], [32, 44]; [41, 2], [41, 3], [41, 5], [41, 11], [41, 27], [41, 30], [41, 43], [41, 44]; [45, 2], [45, 3], [45, 5], [45, 11], [45, 27], [45, 30], [ [45, 43]、[45, 44];[46, 2]、[46, 3]、[46, 5]、[46, 11]、[46, 27]、[46, 30]、[46, 43]、[46, 44];[48, 2]、[48, 3]、[48, 5]、[48, 11]、[48, 27]、[48, 30]、[48, 43]、[48, 44].

[0044] The mobile robot starts at position [1, 1]. The ant colony algorithm parameters are set as follows: number of ants... Maximum number of iterations ; Pheromone importance factor Heuristic function importance factor The local pheromone evaporation factor is 0.5; the global pheromone evaporation factor is... constant coefficients .

[0045] Figure 9 A three-dimensional mountain map provided for an embodiment of the present invention. Figure 9 The corresponding Surf topographic map has the horizontal and vertical axis scales extending from 1 to 50, and the vertical axis scale extending from 0 to 118. The blank areas represent impassable areas. Figure 9 This document is used to display the 3D terrain distribution of the example map. To facilitate problem-solving, it will... Figure 9 The 3D mountain map is converted into Figure 10 A three-dimensional pseudo-color map of the mountainous terrain.

[0046] Figure 10This is a three-dimensional pseudo-color map of mountains provided for an embodiment of the present invention. Figure 10 For a 50×50 grid instance, the horizontal and vertical axes represent the grid positions, with the coordinate scale extending from 1 to 50. The height color scale on the right ranges from approximately 0 to 100, with the color changing from dark blue to yellow indicating a gradual increase in mountain altitude. White squares represent impassable grids. The upper middle area of ​​the map forms a high mountain region, while the middle left area forms a localized elevation region. Impassable grids are distributed across multiple horizontal and vertical positions according to the aforementioned coordinates. Figure 10 Used to display the overall height distribution, obstacle locations, and grid boundaries of the instance map.

[0047] Figure 11 This is an example map grid reachability graph provided in an embodiment of the present invention. Figure 11 The points in the middle correspond to respectively Figure 10 In the feasible grid, the connections between points indicate that adjacent grids are reachable. White empty positions correspond to impassable grids; sparse or broken connections indicate that the elevation difference exceeds the traversable threshold. The area. Figure 11 This shows all the adjacent mesh connections that the ant colony can use in a 50×50 instance, and... Figure 10 The grid positions in the image correspond one-to-one.

[0048] Figure 12 This is a pseudo-color map of the robot's preliminary path provided in an embodiment of the present invention. Figure 12 The x-axis is labeled as coordinate. The vertical axis is labeled as the coordinate axis. The height color scale on the right and Figure 10 Similarly, white grids represent impassable grids, and overlaid path lines represent preliminary paths formed by multiple generations of ant colony pathfinding. The path starts from the lower left region, extends through the center of the map to the upper right region, and includes local turns, short-distance loops, and continuous turns, reflecting the path state before geometric optimization.

[0049] Figure 13 This is a pseudo-color map of the robot's path after removing redundant meshes, provided in an embodiment of the present invention. Figure 13 The coordinate axes, grid height color, and impassable grid distribution are related to Figure 12 Similarly, the superimposed path lines still extend from the lower left region to the upper right region. Figure 12 In contrast, local loops and some intermediate grids that could be directly replaced by adjacent reachable grids have been removed, while the starting position, ending position, and overall direction of travel of the path remain unchanged.

[0050] Figure 14 This is a pseudo-color image of the theoretical mesh path for robot triangulation pruning provided in an embodiment of the present invention. Figure 14The coordinate axes, grid height color, and impassable grid distribution are related to Figure 12 Similarly, the superimposed path lines retain necessary detour segments in the lower left region, form longer diagonal direct connections in the middle of the map, and retain turning segments that satisfy the grid reachability conditions in the upper right region. The diagonal direct connections represent the theoretical node connection relationships obtained from triangulation; the specific feasible grids traversed by the direct connections will be completed in the actual grid path.

[0051] Figure 15 A three-dimensional model of the theoretical mesh path for robot triangular pruning provided in an embodiment of the present invention. Figure 15 The two horizontal axes represent the horizontal and vertical positions of the grid, respectively, while the vertical axis represents the mountain height. The 3D surface represents the mountain surface of the instance map, and the superimposed path lines represent the theoretical grid path. The path lines connect the path nodes retained after triangulation pruning and change with the mountain height corresponding to each retained node, used to display the positional relationship of the theoretical grid path in 3D space and the undulating mountain areas traversed by the path.

[0052] Figure 16 This is a pseudo-color image of the actual mesh path for robot triangulation pruning provided in an embodiment of the present invention. Figure 16 The coordinate axes, grid height color, and impassable grid distribution are related to Figure 14 Similarly, the superimposed path lines represent the actual mesh paths formed after mesh completion of the direct connections in the theoretical mesh paths. Figure 14 The oblique theoretical connection is transformed into a continuous path through a specific grid, and the reachability relationship is satisfied between adjacent path grids. The actual grid path remains within the three-dimensional mountain feasible space.

[0053] Figure 17 A three-dimensional model of the actual mesh path of robot triangulation pruning provided in an embodiment of the present invention. Figure 17 The meanings of the two horizontal and vertical coordinate axes in the diagram are as follows: Figure 15 Similarly, the 3D surface represents the mountain surface of the instance map, and the superimposed path lines represent the actual grid paths. The vertical coordinates of each grid cell in the path correspond to the height of the corresponding mountain, and the path is continuously distributed along the mountain surface; and... Figure 15 compared to, Figure 17 The specific ground-hugging mesh sequence formed after the theoretical node connections are completed is shown.

[0054] Figure 18 A three-dimensional image of the actual curved path of the robot provided in an embodiment of the present invention. Figure 18 The coordinate axes and three-dimensional mountain surface in the middle Figure 17Similarly, the superimposed curved path represents the robot's walking path after curving the inflection points of the polyline in the actual mesh path. The curved path retains the starting position, ending position, and overall direction of the actual mesh path, while the turning positions are different. Figure 17 The polygonal path shown is smoother, which is used to adapt to the robot's turning motion constraints.

[0055] This embodiment also provides a 3D mountain robot path planning system based on ant colony algorithm and geometry optimization.

[0056] Figure 19 A block diagram of a three-dimensional mountain robot path planning system provided in an embodiment of the present invention. Figure 19 The data processing order is shown in the diagram, comprising the construction module, initialization module, preliminary planning module, and path acquisition module, which are connected sequentially by line segments. The processing results from the construction module are passed to the initialization module. The pheromone matrix and potential field heuristic function generated by the initialization module are passed to the preliminary planning module. The preliminary path generated by the preliminary planning module is passed to the path acquisition module.

[0057] The construction module is used to build a grid map, obtain the three-dimensional mountain terrain height numerical matrix, start point and end point, build a pseudo-color map and a SURF map based on the three-dimensional mountain terrain height numerical matrix, determine the reachability relationship between adjacent grids based on the height difference access threshold, and form a three-dimensional mountain feasible space. Figure 10 This represents a 3D pseudo-color map of mountains generated by the building blocks. Figure 11 This indicates the mesh reachability relationships determined by the building module.

[0058] The initialization module is used to determine the initial pheromone matrix and the initial potential field heuristic function of the grid map, and to determine the corresponding numbers of the start node and the end node in the three-dimensional mountain feasible space.

[0059] The preliminary planning module enables ants to select their movement nodes at each step using a roulette wheel approach, based on the initialized pheromone matrix and the initialized potential field heuristic function. It obtains the search paths and path lengths for each ant and forms a preliminary path based on the search paths and path lengths from all iterations. The preliminary planning module also performs tabu list updates, dead-end backtracking, local pheromone dynamic updates, and global pheromone dynamic updates. The preliminary path formed by the preliminary planning module is as follows: Figure 12 As shown.

[0060] The path acquisition module performs geometric optimization on the initial path. Geometric optimization includes redundant mesh removal and triangulation pruning. Based on the triangulation results, a theoretical mesh path is formed. Other meshes that pass sequentially between two directly connected meshes in the theoretical mesh path are then added to form the actual mesh path. This actual mesh path is then transformed into a polyline curve, resulting in a safe, smooth robot walking path that conforms to the 3D mountain terrain. The path after removing redundant meshes is shown below. Figure 13 As shown, the theoretical grid path is as follows Figure 14 and Figure 15 As shown, the actual grid path is as follows Figure 16 and Figure 17 As shown, the robot's walking path after curvature is as follows: Figure 18 As shown.

[0061] The construction module, initialization module, preliminary planning module, and path acquisition module respectively complete the grid environment construction, ant colony parameter initialization, global path search, and path geometry optimization. The processing results are passed between the modules according to the order of data generation to reduce the coupling between different processing stages.

[0062] This embodiment also provides a terminal device. The terminal device includes a processor, a memory, and a computer program stored in the memory and capable of running on the processor. When the processor executes the computer program, it implements the steps of the 3D mountain robot path planning method based on ant colony algorithm and geometry optimization described in this embodiment, or implements the functions of each module in the 3D mountain robot path planning system.

[0063] A computer program can be divided into one or more modules or units, which are stored in memory and executed by a processor to complete the path planning processing of this invention. The terminal device can be a desktop computer, laptop computer, handheld computer, or cloud server. The terminal device includes a processor and memory, and may also be equipped with input interfaces, display interfaces, or communication interfaces as needed for path planning processing.

[0064] A processor can be a central processing unit (CPU), a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), other programmable logic devices, discrete gates, transistor logic devices, or discrete hardware components. The terms CPU, DSP, and FPGA are used interchangeably.

[0065] Memory is used to store computer programs or program modules. The processor implements the path planning function of the terminal device by running or executing the computer programs or program modules stored in memory and calling data in memory.

[0066] When modules or units in a 3D mountain robot path planning system are implemented as software functional units and used as independent products, they can be stored in a computer-readable storage medium. The computer-readable storage medium stores a computer program, which, when executed by a processor, implements all or part of the steps of the 3D mountain robot path planning method based on ant colony algorithm and geometry optimization described in this embodiment.

[0067] Computer program code can take the form of source code, object code, executable files, or intermediate code. Computer-readable storage media can include USB flash drives, external hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), and random access memory (RAM). Read-only memory (ROM) and random access memory (RAM) are physical media or devices capable of storing computer program code, excluding electrical carrier signals and telecommunication signals themselves.

[0068] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit the scope of protection of the present invention. Any adjustments or substitutions made by those skilled in the art to the specific embodiments based on the present invention specification, as long as they do not depart from the essence of the technical solutions of the present invention, should fall within the scope of protection of the present invention.

[0069] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the systems disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple; relevant parts can be referred to the method section.

[0070] Specific examples have been used to illustrate the principles and implementation methods of this invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of this invention. Furthermore, those skilled in the art will recognize that, based on the ideas of this invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this invention.

Claims

1. A path planning method for a 3D mountain robot based on ant colony optimization and geometry optimization, characterized in that, include: Obtain the three-dimensional mountain terrain height numerical matrix, starting point and ending point. Determine the reachability relationship between adjacent grids based on the comparison result of the mountain height difference between adjacent grids and the height difference access threshold. Eliminate inaccessible grids with abrupt height changes to form a three-dimensional mountain feasible space composed of feasible grids and the reachability relationship between adjacent feasible grids. Determine the feasible grids corresponding to the starting point and the ending point. A pheromone matrix is ​​initialized for the three-dimensional mountain feasible space, and a potential field heuristic function is constructed based on the three-dimensional straight-line distance from each feasible grid in the three-dimensional mountain feasible space to the feasible grid corresponding to the termination point. Based on the pheromone matrix and the potential field heuristic function, a multi-generation ant colony path search is performed from the feasible grid corresponding to the starting point to the feasible grid corresponding to the ending point. Local pheromone dynamic updates are performed during each generation of search, and global pheromone dynamic updates are performed after each generation of search. The search path with the shortest path length in all iterations is determined as the initial path. Redundant meshes are removed from the initial path, and triangulation is performed on the path after removing redundant meshes. When the direct connection between the first and last meshes of a polyline formed by multiple continuous meshes passes through feasible meshes in sequence, and the adjacent meshes passed through by the direct connection in sequence are all reachable, and the actual path length of the direct connection is less than the actual path length of the polyline, the polyline is replaced by the direct connection between the first and last meshes, and the triangulation is repeated along the path sequence to form a theoretical mesh path. Complete the other grids that are sequentially passed between two directly connected grids in the theoretical grid path to form an actual grid path composed of adjacent reachable grids in the three-dimensional mountain feasible space; The inflection points of the polyline in the actual grid path are converted into polyline curves to form the robot's walking path.

2. The 3D mountain robot path planning method based on ant colony algorithm and geometry optimization according to claim 1, characterized in that, Obtain the three-dimensional mountain terrain height numerical matrix, starting point, and ending point. Determine the reachability relationship between adjacent grids based on the comparison between the mountain height difference and the height difference access threshold. Eliminate inaccessible grids with abrupt height changes to form a three-dimensional mountain feasible space composed of feasible grids and the reachability relationships between adjacent feasible grids. Determine the feasible grids corresponding to the starting point and the ending point, including: The mountain height of the corresponding grid is determined according to each element in the three-dimensional mountain terrain height numerical matrix. The corresponding grid is colored according to the mountain height of each grid to form a pseudo-color map. Grids that do not have a determined height value are excluded from the feasible grids. Calculate the mountain height difference between adjacent grids, determine adjacent grids whose mountain height difference is not greater than the height difference access threshold as reachable, and determine adjacent grids whose mountain height difference is greater than the height difference access threshold as inaccessible; The three-dimensional mountain feasible space is formed based on the feasible grid and the reachability relationships between adjacent feasible grids; Determine whether the starting point and the ending point are located within the terrain boundary range corresponding to the three-dimensional mountain terrain height numerical matrix, and determine whether the grids corresponding to the starting point and the ending point have a definite height value. If the determination results are both yes, determine the feasible grids corresponding to the starting point and the ending point.

3. The 3D mountain robot path planning method based on ant colony algorithm and geometry optimization according to claim 1, characterized in that, A pheromone matrix is ​​initialized for the three-dimensional mountain feasible space, and a potential field heuristic function is constructed based on the three-dimensional straight-line distance from each feasible grid in the three-dimensional mountain feasible space to the feasible grid corresponding to the termination point, including: The pheromone matrix is ​​established based on the reachability relationship between adjacent feasible grids in the three-dimensional mountain feasible space, so that each element in the pheromone matrix represents the pheromone concentration between the corresponding adjacent feasible grids. Assign initial values ​​to the pheromone concentration between adjacent feasible grids, and set a minimum pheromone value; Based on the differences in abscissa, ordinate, and mountain height between each feasible grid in the three-dimensional mountain feasible space and the feasible grid corresponding to the termination point, calculate the three-dimensional straight-line distance from each feasible grid to the feasible grid corresponding to the termination point. The reciprocal of the three-dimensional straight-line distance corresponding to each feasible grid other than the feasible grid corresponding to the termination point is determined as the heuristic factor of the corresponding feasible grid. The heuristic factor of the feasible grid corresponding to the termination point is set to a preset value, and the potential field heuristic function is constructed according to the heuristic factor of each feasible grid.

4. The 3D mountain robot path planning method based on ant colony algorithm and geometry optimization according to claim 1, characterized in that, Based on the pheromone matrix and the potential field heuristic function, a multi-generation ant colony path search is performed from the feasible grid corresponding to the starting point to the feasible grid corresponding to the ending point, including: In each generation of ant colony path search, each ant starts moving from the feasible grid corresponding to the starting point, and a taboo table is created for each ant to record the grids it has passed. Exclude grids that are already recorded in the taboo list of the current ant from the adjacent reachable grids of the current grid to form a set of accessible grids; For each grid in the accessible grid set, calculate the product of the power of the pheromone importance parameter of the pheromone concentration between the current grid and the corresponding grid and the power of the power of the heuristic factor importance parameter of the corresponding grid. The ratio of the product to the sum of all products in the accessible grid set is determined as the grid transition probability of the corresponding grid. The next moving grid for the current ant is selected by roulette wheel selection based on the grid transfer probability, and the next moving grid is recorded in the current ant's search path and taboo table; When the set of accessible grids is empty, the current ant returns to the previous grid in the search path, records the current grid before returning in the tabu list of the current ant, and reconstructs the set of accessible grids based on the current grid after returning. Repeat the selection of the next moving grid until the current ant reaches the feasible grid corresponding to the termination point.

5. The 3D mountain robot path planning method based on ant colony algorithm and geometry optimization according to claim 1, characterized in that, Local pheromone dynamic updates are performed during each search cycle, including: In the same generation of ant colony path search, after each ant completes the path search from the feasible grid corresponding to the starting point to the feasible grid corresponding to the ending point, it reduces the pheromone concentration between adjacent reachable grids traversed by the corresponding search path according to the local pheromone evaporation factor. The pheromone matrix is ​​updated based on the reduced pheromone concentration, and the updated pheromone matrix is ​​used to determine the grid transfer probability for ants in the same generation that have not yet completed path search.

6. The 3D mountain robot path planning method based on ant colony algorithm and geometry optimization according to claim 1, characterized in that, After each generation of search, a global pheromone dynamic update is performed, and the search path with the shortest path length in all iterations is determined as the initial path, including: After all ants in each generation have completed the path search from the feasible grid corresponding to the starting point to the feasible grid corresponding to the ending point, the original pheromone concentration in the pheromone matrix is ​​reduced according to the global pheromone evaporation factor. Based on the search path of each ant, pheromone is added between adjacent reachable grids traversed by the corresponding search path. The amount of pheromone added is the ratio of the pheromone increase intensity coefficient to the total path length of the corresponding search path. The pheromone matrix is ​​updated based on the pheromone concentration after pheromone volatilization and pheromone increase, thus completing the global pheromone dynamic update. Record the search path and corresponding path length of each ant in all iterations, and determine the search path with the shortest path length as the initial path.

7. The 3D mountain robot path planning method based on ant colony algorithm and geometry optimization according to claim 1, characterized in that, Redundant mesh deletion is performed on the initial path, including: Three consecutive grids are selected sequentially along the path of the initial path, and these three consecutive grids are sequentially designated as the first grid, the middle grid, and the last grid. Determine whether the first grid and the last grid are adjacent reachable grids; When the first grid and the last grid are adjacent reachable grids, delete the intermediate grid and directly connect the first grid and the last grid; when the first grid and the last grid are not adjacent reachable grids, retain the intermediate grid. Continue selecting the next set of three consecutive grids along the current path, and repeat the above judgment until no intermediate grids are deleted in a complete traversal of the current path.

8. The 3D mountain robot path planning method based on ant colony algorithm and geometry optimization according to claim 1, characterized in that, Triangulation pruning is performed on the paths after removing redundant meshes, including: Three consecutive grids are selected sequentially along the path after removing redundant grids. These three consecutive grids are then designated as the first grid, the middle grid, and the last grid. The polyline path formed by connecting the first grid, the middle grid, and the last grid in sequence is then designated as the multi-segment polyline. Calculate the three-dimensional Euclidean distance between the first grid and the middle grid, and between the middle grid and the tail grid, respectively, and determine the sum of the two three-dimensional Euclidean distances as the actual path length of the polyline; The grids that the direct connection between the first grid and the last grid passes through in sequence are determined. The three-dimensional Euclidean distances between adjacent grids that the direct connection passes through in sequence are calculated respectively, and the sum of the three-dimensional Euclidean distances is determined as the actual path length of the direct connection. When all the grids traversed by the direct connection in sequence are feasible grids, all adjacent grids traversed by the direct connection in sequence are reachable, and the actual path length of the direct connection is less than the actual path length of the polyline, the polyline is replaced by the direct connection between the first grid and the last grid; when any of the conditions are not met, the polyline is retained. Continue selecting the next set of three consecutive grids along the current path sequence, and repeat the triangulation until no replacement occurs in a complete traversal of the current path, thus forming the theoretical grid path.

9. The 3D mountain robot path planning method based on ant colony algorithm and geometry optimization according to claim 1, characterized in that, To complete the theoretical grid path by sequentially traversing other grids between two directly connected grids, forming an actual grid path composed of adjacent reachable grids within the three-dimensional feasible mountain space, including: Along the path sequence of the theoretical grid path, select two grids that are adjacent in path sequence in turn; Determine the direct connection between two adjacent grids in the path sequence and other grids traversed sequentially within the three-dimensional mountain feasible space; According to the order in which the other grids pass through the direct connection, the other grids are inserted between two grids that are adjacent in the path order; The mesh completion process is repeated along the path sequence of the theoretical mesh path until all groups of meshes with adjacent paths in the theoretical mesh path are completed, forming the actual mesh path composed of adjacent reachable meshes within the three-dimensional mountain feasible space.

10. The 3D mountain robot path planning method based on ant colony algorithm and geometry optimization according to claim 1, characterized in that, The inflection points of the polyline in the actual grid path are converted into polylines to form the robot's walking path, including: Starting from the second grid in the actual grid path, select the current grid and the preceding and following grids that are adjacent to the current grid in the path order in sequence; Compare the line segment direction from the previous grid to the current grid with the line segment direction from the current grid to the next grid. When the two line segment directions are not the same, the current grid is determined as the inflection point of the polyline. The polyline segment formed by sequentially connecting the previous grid, the polyline inflection point, and the next grid is replaced by a curve segment connecting the previous grid and the next grid. When two line segments have the same direction, retain the polyline segment formed by connecting the previous grid, the current grid, and the next grid in sequence; The robot continues to select subsequent grids along the path sequence of the actual grid path until the processing of the grids before the feasible grid corresponding to the termination point is completed, thus forming the robot's walking path.

Images