Route planning method of rugged environment improved A* algorithm based on elevation map

Abstract

The invention discloses a rugged environment improved A * algorithm path planning method based on an elevation map, and relates to the technical field of path planning. The method comprises the following steps: establishing a ground trafficability model and a ground rugged model which conform to the characteristics of a rugged environment terrain based on an elevation map, further identifying the passing difficulty and wave degree of the ground, and then designing an elevation cost function capable of obtaining an optimal path according to the two types of models in combination with the main process of an A * algorithm. And finally, a continuous cost function is established in combination with an original distance cost function, the path optimality is improved, and an optimal route is selected. The improved algorithm enhances the stability and optimality of the planned path of the mobile robot in the rugged environment, and improves the autonomous navigation performance of the mobile robot in the rugged environment.

Description

Technical Field

[0001] The invention belongs to the technical field of path planning, and in particular relates to a path planning method for a rugged environment based on an elevation map and an improved A* algorithm. Background Art

[0002] Mobile robot path planning is a research hotspot in the robotics field. Its purpose is to search for an optimal or near-optimal safe collision-free path from the start node to the target node in an unknown or known environment according to the specified optimization rules. At present, a lot of research has been done on mobile robot path planning algorithms at home and abroad, and many results have been achieved. However, most of them are applied in smooth ground environments. When applied in rugged environments, the mobile robot often suffers from the problem of instability of the center of mass due to the unstable road surface.

[0003] In response to the above problems, the current mobile robot uses an improvement on the occupancy grid method, replacing the original occupancy data with a 2.5-dimensional elevation map of elevation values ​​to describe the ruggedness of the ground, thereby avoiding ground instability. However, the existing improved path planning algorithm based on elevation maps only considers the stability of the ground within a single grid. When the volume of the mobile robot itself passes through multiple grids, it cannot reflect the stability of the mobile robot. In addition, the cost of the elevation of each node in the grid in the existing algorithm is only a method of classifying the ruggedness of the ground using fuzzy logic to assign costs, resulting in the algorithm generating a path that is not optimal. Due to the above two types of problems, the existing path planning algorithm based on elevation maps generates poor paths, which leads to poor navigation performance of the mobile robot in rugged environments and the problem of center of mass instability.

[0004] Therefore, it is necessary to propose a path planning method for improving the A* algorithm in rugged environments based on elevation maps to solve the above two problems. Summary of the invention

[0005] Therefore, the technical problem to be solved by the present invention is to avoid the problem of center of mass instability when the mobile robot autonomously navigates in a rugged environment.

[0006] To achieve the above object, the present invention provides the following technical solution, comprising the following steps:

[0007] S1. First enter the starting point T start With target T goal , initialize the algorithm parameters, initialize Openlist and Closelist, and set T goal Add to Openlist;

[0008] S2. driving the mobile robot and establishing an elevation map based on the sensor information obtained by the mobile robot;

[0009] S3. Expand the node according to the starting point of the mobile robot and output the child node;

[0010] S4. Establish a ground passability model and a ground ruggedness model based on the starting point and the output node, and obtain the slope a, elevation difference h, and ground ruggedness S of the node.

[0011] S5. Calculate the cost function G through the slope a, elevation difference h, and ground roughness S of the node g (n), and weighted to obtain the node cost F(n);

[0012] S6. Get the node T with the minimum node cost F(n) m , with T m is the parent node, traverse the child nodes T k , and make judgments and comparisons, select the point with the best cost, and use it as the new starting point;

[0013] S7. Repeat steps S3-S6 until the end point is found, or if the end point cannot be found, it is considered that there is no feasible path.

[0014] Preferably, the ground passability model and the ground roughness model in S4 are obtained through the parent node T start The coordinates (x0, y0, z0) of the search node are j The coordinates (x i ,y j , z i，j ), these two types of models are mainly based on the grid center coordinates of the elevation map to further simulate the undulation of the ground, thereby simulating the difficulty of the mobile robot in driving and the local stability, and then reflecting the quality of the planned route. The ground passability model mainly describes the difficulty of the mobile robot passing through the ground in the route. It contains two models, namely the slope model and the elevation difference model. The elevation difference model and the slope model are respectively

[0015] h=z i，j -z0 (1)

[0016]

[0017] The ground roughness reflects the ground fluctuation of the local ground, and thus reflects the stability of the mobile robot on this path. The formula of the ground roughness model is:

[0018]

[0019] where z i-1，j-1 T jThe elevation of the surrounding grids can reflect the degree of ground fluctuation through the ground ruggedness model, and further reflect the stability of the mobile robot passing through the ground.

[0020] Preferably, the elevation cost G in S5 g (n) By the slope cost function f p , elevation difference cost function f e , the roughness cost function f q The cost functions are composed of three types, and the corresponding weights are ω p ,ω e ,ω q , where ω p ,ω e ,ω q The sum of is 1.

[0021]

[0022]

[0023] The slope cost function and elevation difference cost function are shown in Equation 4 and Equation 5, respectively, where h and a are the parent node T start The coordinates (x0, y0, z0) of the search node are j The coordinates (x i ,y j , z i，j ) elevation and slope, h y , a y are the elevation difference and slope thresholds of the mobile robot respectively. The ruggedness cost function is shown in Equation 6, where S is the ground ruggedness of the search node, S y is the ground roughness threshold of the mobile robot. The final weighted G(n) is shown in Formula 7.

[0024]

[0025] G(n)=ω1G d (n)+ω2G g (n) (7)

[0026] The final weighted F(n) is shown in Formula 8

[0027] F(n)=G(n)+H(n) (8)

[0028] Compared with the prior art, the advantages of the present invention are:

[0029] The present invention first designs a corresponding ground passability model and ground ruggedness model based on the elevation map according to the characteristics of the rugged environment, so as to reflect the ground passability and smoothness of the search path of the mobile robot; then, a continuous cost function that conforms to the characteristics of the rugged environment is designed according to the ground passability model and the ground ruggedness model, thereby improving the optimality of the mobile robot's path planning in the rugged environment. Through the above two types of improvements, the mobile robot can search for the optimal stable path, thereby avoiding the instability of the center of mass of the mobile robot's autonomous navigation in the rugged environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] In order to fully illustrate the specific content of the present invention, the present invention is further described in detail below according to the specific implementation of the present invention and in conjunction with the accompanying drawings, wherein

[0031] Figure 1 Flowchart of improving the A* algorithm for rugged environments based on elevation maps.

[0032] Figure 2 Search the directional graph for child nodes.

[0033] Figure 3 This is the elevation difference model map.

[0034] Figure 4 This is the slope model diagram.

[0035] Figure 5 This is a model diagram of ground roughness.

[0036] Figure 6 These are simulation experiment diagrams in rugged environments, where (a) is a simulation experiment diagram of the improved A* algorithm in a rugged environment based on an elevation map; (b) is a simulation experiment diagram of the A* algorithm; and (c) is a simulation experiment diagram of the A* algorithm with obstacle expansion added. DETAILED DESCRIPTION

[0037] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0038] First combine Figure 1 , the specific implementation is as follows:

[0039] S1. Input starting point T start With target T goal ,Path planning requires the initial position and the target position to be obtained before the algorithm can start.

[0040] S2. Initialize Openlist and Closelist, and set T goalAdd to Openlist. Since it is necessary to record the reachable nodes and the nodes that have been reached, Openlist and Closelist are established to store the reachable nodes and the nodes that have been reached.

[0041] S3. Initialization parameter ω p ,ω e ,ω q ,ω1,ω2,h y , a y , S y . Among them ω p ,ω e ,ω q , ω1, ω2 are the elevation cost function G in the algorithm respectively. g (n) The weights of the three cost functions: slope cost function, elevation difference cost function, and ruggedness cost function, and h y , a y , S y They represent the elevation difference, slope, and ground roughness threshold of the mobile robot in the algorithm. If the threshold is exceeded, it is considered that the ground is impassable or the ground fluctuates greatly, causing the center of mass of the mobile robot to become unstable.

[0042] S4. Create an elevation map for T start Expand the node and output the child node Tj. The mobile robot creates an elevation map of the mobile robot using the point cloud information obtained by the sensor. The elevation map moves with the movement of the robot and stores the elevation values ​​of the grid where the mobile robot is located and the surrounding grids. It also merges the existing data with the new measurement data to form a global elevation map. Figure 2 As shown, the center of the grid where the mobile robot is located is used as the initial position node, and the centers of the eight-directional grids around the current grid where the mobile robot is located are used as child nodes for searching.

[0043] S5. Create child node T j The ground passability model and ground roughness model are outputted as T j h,a,s.

[0044] The ground passability model is a model that describes the difficulty of a mobile robot passing through a path formed between a parent node and a child node. The environments that are difficult for existing mobile robots to pass through include inclined ground in rugged environments, independent protruding ground in rugged environments, and sunken ground including steps, equipment, and sunken potholes. Therefore, in view of the above characteristics, an elevation difference model and a slope model will be established through an elevation map to determine whether the searched ground is passable and the difficulty of passing. The ground passability model includes two models, namely the slope model and the elevation difference model, in which the elevation difference model and the slope model are as follows: Figure 3 , as shown in 4, the calculation formula is

[0045] h=z i，j -z0 (9)

[0046]

[0047] The ground roughness reflects the ground fluctuation of the local ground, and thus reflects the stability of the mobile robot on this path. The ground roughness model is as follows: Figure 5 As shown, the calculation formula is

[0048]

[0049] where z i-1，j-1 T j The elevation of the surrounding grids can reflect the degree of ground fluctuation through the ground ruggedness model, and further reflect the stability of the mobile robot passing through the ground.

[0050] S6. Calculate the elevation cost G g (n), and then weighted to obtain T j The cost F(n). Since the ground passability and ground ruggedness are mainly determined by the elevation of each grid, G(n) in the A* algorithm is the evaluation cost value from the starting node to the current node, and H(n) is the predicted evaluation cost value from the current node to the target node. Among them, H(n) is a heuristic function, which is mainly used to improve the search efficiency, so G(n) will be improved to improve the optimality of the path. Therefore, an elevation cost function with a continuous cost is designed, which takes into account the normal navigation threshold of the mobile robot and the characteristics that the difficulty and fluctuation of the rugged environment increase faster and faster with the increase of the value, to describe the difficulty and fluctuation of the navigation path of the mobile robot in the rugged environment. The elevation cost G g (n) By the slope cost function f p , elevation difference cost function f e , the roughness cost function f q The cost functions are composed of three types, and the corresponding weights are ω p ,ω e ,ω q , where ω p ,ω e ,ω q The sum of is 1.

[0051]

[0052]

[0053] The slope cost function and elevation difference cost function are shown in Equation 4 and Equation 5, respectively, where h and a are the parent node T startThe coordinates (x0, y0, z0) of the search node are j The coordinates (x i ,y j , z i，j ) elevation and slope, h y , a y are the elevation difference and slope thresholds of the mobile robot respectively. The ruggedness cost function is shown in Equation 6, where S is the ground ruggedness of the search node, S y is the ground roughness threshold of the mobile robot. The final weighted G(n) is shown in Formula 7.

[0054]

[0055] G(n)=ω1G d (n)+ω2G g (n) (15)

[0056] The final weighted F(n) is shown in Formula 8

[0057] F(n)=G(n)+H(n) (16)

[0058] S7. Calculate the node T with the smallest F in Openlist m ,By calculating the existing path nodes, we calculate the nodes with the minimum cost to ensure the optimality of the route.

[0059] S8.T m is the parent node, traverse the child nodes T k , and make a judgment. First, make a judgment T k Is it in Openlist? If T k In Openlist, indicate T k has been reached, so it is not the optimal point, so ignore this point; if T k If it is not in the Openlist, it means T k It has not been reached yet, continue to judge.

[0060] Then judge T k Is it in the Closelist? If T k If it is not in the Closelist, it means T m If it is not a node that has been reached, it can be said that the path is optimal, and then T k Add to Openlist, m Transfer from Openlist to Closelist; if T k In Closelist, determine whether to use T m Is the node's G better than the original G?m If the G of a node is not better than the original G, then ignore the node. m If the node's G is better than the original G, then T m The node is the optimal node, so T k Add to Openlist, m Move from Openlist to Closelist.

[0061] S9. Judge T goal Whether the Closelist or Openlist is empty, if the above conditions are met, the program ends and the optimal path is output; if not, S7-S8 are repeated.

[0062] In order to verify the advantages of the path planning method for rugged environment improved A* algorithm based on elevation map proposed in the present invention, the original A* algorithm, the A* algorithm with obstacle expansion and the improved A* algorithm for rugged environment based on elevation map were run 10 times in the rugged environment, and the total path distance, average slope of each node, average elevation difference of each node and average ground ruggedness of each node obtained in each run were counted. The data obtained in the rugged environment are shown in Table 1 below. The simulation experiment results are shown in Table 1 below. Figure 6 shown.

[0063] The simulation results show that in the same rugged environment, the average value of the slope formed by each node in the path, the average value of the elevation difference formed by each node in the path, and the ground ruggedness of each node in the path of the improved A* algorithm are much lower than those of the A* algorithm. Therefore, it is proved that when the improved algorithm is applied, the mobile robot will choose easy-to-pass ground and relatively smooth ground in the rugged environment. Compared with the A* algorithm with added obstacle expansion, the improved A* algorithm reduces the total path length by 20.8% while maintaining the average value of the slope formed by each node in the path, the average value of the elevation difference formed by each node in the path, and the ground ruggedness of each node in the path. The optimality of the path of the improved A* algorithm is verified. Therefore, the improved A* algorithm based on the rugged environment of the elevation map has good performance in terms of distance, ground passability and ground ruggedness, which verifies the effectiveness of the algorithm and proves that the algorithm has good performance in the rugged environment.

[0064] The technical contents not described in detail in the present invention are all known technologies.

[0065] Table 1 Comparison of algorithm performance in rugged environments

[0066]

Claims

1. A path planning method for a rugged environment based on an improved A* algorithm based on an elevation map, characterized in that: The path planning method comprises the following steps: S1. First enter the starting point T start With target T goal , initialize the algorithm parameters, initialize Openlist and Closelist, and set T goal Add to Openlist; S2. driving the mobile robot and establishing an elevation map based on the sensor information obtained by the mobile robot; S3. Expand the node according to the starting point of the mobile robot and output the child node; S4. Establish a ground passability model and a ground ruggedness model based on the starting point and the output node, and obtain the slope a, elevation difference h, and ground ruggedness S of the node. S5. Calculate the cost function G through the slope a, elevation difference h, and ground roughness S of the node g (n), and weighted to obtain the node cost F(n); S6. Get the node T with the minimum node cost F(n) m , with T m is the parent node, traverse the child nodes T k , and make judgments and comparisons, select the point with the best cost, and use it as the new starting point; S7. Repeat S3-S6 until the end point is found, or if the end point cannot be found, it is considered that there is no feasible path.

2. The path planning method of the improved A* algorithm in a rugged environment based on an elevation map as claimed in claim 1, characterized in that: The ground passability model and ground roughness model in S4 are constructed as follows. The ground passability model and ground roughness model are constructed through the parent node T start The coordinates (x0, y0, z0) of the search node are j The coordinates (x i ,y j , z i，j ), these two types of models are mainly based on the grid center coordinates of the elevation map to further simulate the undulation of the ground, thereby simulating the difficulty of the mobile robot in driving and the local stability, and then reflecting the quality of the planned route. The ground passability model mainly describes the difficulty of the mobile robot passing through the ground in the route. It contains two models, namely the slope model and the elevation difference model. The elevation difference model and the slope model are respectively h=z i，j -z0 (1) The ground roughness reflects the ground fluctuation of the local ground, and thus reflects the stability of the mobile robot on this path. The formula of the ground roughness model is: where z i-1，j-1 T j The elevation of the surrounding grids can reflect the degree of ground fluctuation through the ground ruggedness model, and further reflect the stability of the mobile robot passing through the ground.

3. The path planning method of the improved A* algorithm in a rugged environment based on an elevation map as claimed in claim 1, characterized in that: In S5, construct G as follows g (n), the elevation cost G in S5 g (n) By the slope cost function f p , elevation difference cost function f e , the roughness cost function f q The cost functions are composed of three types, and the corresponding weights are ω p ,ω e ,ω q , where ω p ,ω e ,ω q The sum of is 1. The slope cost function and elevation difference cost function are shown in Equation 4 and Equation 5, respectively, where h and a are the parent node T start The coordinates (x0, y0, z0) of the search node are j The coordinates (x i ,y j , z i，j ) elevation and slope, h y , a y are the elevation difference and slope thresholds of the mobile robot respectively. The ruggedness cost function is shown in Equation 6, where S is the ground ruggedness of the search node, S y is the ground roughness threshold of the mobile robot. The final weighted G(n) is shown in Formula 7. G(n)=ω1G d (n)+ω2G g (n) (7) The final weighted F(n) is shown in Formula 8 F(n)=G(n)+H(n) (8).

4. The path planning method of the improved A* algorithm in a rugged environment based on an elevation map according to claim 1, characterized in that: The algorithm is applicable to rugged environments, including disaster sites, mountainous hills, and planetary surfaces.

Images