A multi-constraint multi-level path planning method

Abstract

The present application relates to a kind of multi-constraint multilevel path planning method, to solve the problem that the flexibility of path planning is low, and optimal path cannot be accurately found in the prior art path planning method only considering the obstruction of passage type obstacle, method includes the following steps: obtaining multi-layer map information, path planning starting point and target point position;Each layer map information includes the passage type and passage cost multiplier of each passage node in the layer map;The passage type includes normal passage type, slow passage type and obstruction passage type;According to the passage type and passage cost multiplier of each passage node in each layer map, the optimal path from starting point to target point is calculated using improved path planning algorithm.Make the optimal path result more accurate, and the path cost of planning place is shorter.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of path planning, in particular to a multi-constraint multi-level path planning method. BACKGROUND

[0002] In real and virtual world, such as car navigation and game task guidance, we are always solving a common problem in different fields, how to reach the destination from the starting point with the shortest path, of course, the straight line from the starting point to the destination will be the shortest distance, but this does not take into account the "passability" of the path. It is the limitation of passability that leads to the fact that in the actual implementation process, it cannot completely follow the straight line to the target. The reasons for the decline or disappearance of "passability" mainly include two points, one is the obstacle to passing, and the other is the slow passing.

[0003] The obstacle to passing is an obstacle that cannot be passed directly, including natural obstacles such as cliffs, peaks, etc., and man-made obstacles such as buildings, road controls, etc. The only way to pass such obstacles is to detour, which will also increase the path cost of passing. The slow passing obstacle is an obstacle that will significantly reduce the passing speed, including natural obstacles such as rivers, bogs, etc., and man-made obstacles such as speed limits, flow limits, etc. Such obstacles will reduce the passing efficiency, and even change the passing method, such as changing from land to water. Especially when the cost of slow passing increases too much, slow passing can be approximately regarded as part of the obstacle to passing, because it also forms a result that must be detoured.

[0004] Cost and price are the key indicators for describing the advantages and disadvantages of a planned path in path planning. "Cost" and "price" are equivalent in semantics, and are collectively referred to as price below. The lower the path price is, the shorter the total distance of the planned path is. When assuming that the speed of completing different paths is consistent, the corresponding time of the path with the lowest price is also the shortest. Generally, the path with the shortest price is called the optimal path, and the optimal path has more time or distance advantages and should be chosen by the user. Of course, the goal of the price is not necessarily to achieve the shortest path, but also to achieve the shortest time or the lowest fuel consumption. In order to be more intuitive, the length of the path is usually selected as the most basic price function target.

[0005] In the existing path determination method, only the obstacle of obstructing traffic is considered, i.e. when the obstacle is encountered, the "detour" processing is needed, the related consideration of reducing the obstacle of obstructing traffic is lacked, when the obstacle of reducing the traffic is introduced, the cost increased by the "detour" can be far more than the cost of crossing the complex obstacle, such as crossing the river; meanwhile, the existing path determination method is limited in the two-dimensional plane, i.e. the height dimension or the level dimension information is not considered, such as in the reality, the lack of the dimension information will inevitably reduce the flexibility and the optimality of the path planning. SUMMARY

[0006] In view of the above analysis, the embodiments of the present application aim to provide a multi-constraint multi-level path planning method to solve the problem that the existing path planning method only considers the obstacle of obstructing traffic and lacks the Z-axis information, the flexibility of the path planning is low, and the optimal path cannot be accurately found.

[0007] In one aspect, the embodiments of the present application provide a multi-constraint multi-level path planning method, comprising the following steps:

[0008] obtaining the multi-level map information, the starting point and the target point position of the path planning; each level of map information comprises the traffic type and the traffic cost multiplier of each traffic node in the map of the level; the traffic type comprises the normal traffic type, the reduced traffic type and the obstructed traffic type;

[0009] According to the traffic type and the traffic cost multiplier of each traffic node in each level of map, an improved path planning algorithm is used to calculate the optimal path from the starting point to the target point.

[0010] Based on the further improvement of the above technical solution, according to the traffic type and the traffic cost multiplier of each traffic node in each level of map, an improved path planning algorithm is used to calculate the optimal path from the starting point to the target point, comprising:

[0011] S21, putting the starting point into the intention node set;

[0012] S22, selecting the node with the minimum total cost value in the intention node set as the current parent node, and moving the current parent node to the visited node set and the planning node set;

[0013] S23, according to the traffic type and the traffic cost multiplier of each node, calculating the total cost value of the node in the adjacent nodes of the current parent node which is not in the visited node set and is not of the obstructed traffic type; adding the node in the adjacent nodes of the current parent node which is not in the visited node set and is not of the obstructed traffic type to the intention node set; the total cost value of the node is the sum of the approach cost value from the starting point to the current node through the parent node and the estimated cost value from the current node to the target node;

[0014] S24, if the target node is contained in the adjacent nodes of the current parent node, moving the target node to the visited node set and the planning node set; in the planning node set, the planning path is found according to the parent-child relationship between the nodes;

[0015] S25, otherwise, returning to step S22.

[0016] Further, for each adjacent node p i (x,y) of the current node which is not in the visited node set and is not of the obstructing passing type

[0017] If the node p i (x,y) is not in the intended node set, directly adding the node p i (x,y) to the intended node set, and recording the total cost, the path cost, the estimated cost of the node p i (x,y) and the parent node of the node p i (x,y) as the current parent node;

[0018] If the node p i (x,y) is in the intended node set, if the path cost of the node p i (x,y) calculated according to the current parent node is less than the estimated cost of the node p i (x,y) recorded in the intended node set, updating the parent node of the node p i (x,y) in the intended node set as the current parent node, and updating the total cost and the path cost of the node p i (x,y) as the total cost and the path cost calculated according to the current parent node.

[0019] Further, selecting the node with the minimum total cost in the intended node set as the current parent node, comprising:

[0020] sequentially sorting the nodes in the intended node set according to the total cost from small to large; if the total costs of the nodes are the same, sequentially sorting the nodes according to the estimated cost from the target node from small to large, if the estimated costs are the same, sequentially sorting the nodes according to the order from the last to the first when the nodes are added to the intended node set, and taking the intended node in the first order after the sorting as the current parent node.

[0021] Further, the adjacent nodes of the current parent node include the adjacent nodes in the same layer as the current parent node, and the adjacent nodes in the adjacent layer as the current parent node.

[0022] Further, if the adjacent node is in the same layer as the parent node, calculating the path cost by the following formula

[0023]

[0024] If neighboring nodes and parent nodes are on different layers, the path cost is calculated using the following formula.

[0025]

[0026] in, This represents the path cost of the parent node located at level i. This represents the path cost of the parent node located at level j. If the parent node is the starting node, then the path cost of the parent node is 0. Let represent the path cost to neighboring nodes in layer i, and d represent the inter-layer switching cost. This represents the cost from the parent node to the neighboring node.

[0027] Furthermore, according to the formula Calculate the cost from the parent node to neighboring nodes.

[0028] Where l represents the cost multiplier of neighboring nodes, λ represents the cost per unit distance traveled, (x c ,y c (x) represents the planar coordinates of neighboring nodes. f ,y f ) represents the planar coordinates of the parent node.

[0029] Furthermore, when neighboring nodes and the target node are at the same layer, the estimated cost is calculated using the following formula.

[0030] in, D m =|x c -x e |+|y c -y e |,λ represents the cost per unit distance traveled, (x c ,y c (x) represents the planar coordinates of neighboring nodes. e ,y e ) represents the planar coordinates of the target node, and w1 and w2 represent the weight coefficients.

[0031] Furthermore, when neighboring nodes and the target node are at different layers, the estimated cost is calculated using the following formula.

[0032] in, D m =|x c -x e |+|yc -y e |, lambda represents the value of generation across unit distance, (x c ,y c ) represents the plane coordinates of adjacent nodes, (x e ,y e ) represents the plane coordinates of target nodes, w1 and w2 represent weight coefficients, d represents the interlayer switching cost, i represents the serial number of the layer where the adjacent node is located, and j represents the serial number of the layer where the target node is located.

[0033] Further, in the planning node set, the planning path is found according to the parent-child relationship between nodes, including:

[0034] S241, taking the target node as the current node;

[0035] S242, adding the current node to the path node set; if the current node is the starting node, the finding is stopped, and the node sequence in the path node set is the planned path;

[0036] S243, otherwise, finding the parent node of the current node in the planning node set, taking the parent node of the current node as the current node, and returning to step S242.

[0037] Compared with the prior art, the dimension information of the Z axis is considered in the application, and the Z axis is divided into a normal passing type, a slow passing type and an obstructing passing type according to the passing type, so that the path planning is no longer only detouring, and the path can also be planned in the Z axis space, so that different types of obstacles can be flexibly avoided, a more optimal path can be selected in different levels, and the optimal path result is more accurate. The method is more adaptable to the environment with obstacles, and the path cost of planning is shorter.

[0038] In the application, the above technical solutions can be combined with each other to realize more preferred combination solutions. Other features and advantages of the application will be described in the subsequent specification, and some advantages will become apparent from the specification or by implementing the application. The purpose and other advantages of the application can be realized and obtained from the contents specifically pointed out in the specification and the drawings. BRIEF DESCRIPTION OF DRAWINGS

[0039] The accompanying drawings are included to provide a further understanding of the application and are incorporated herein and constitute a part of the application. The drawings illustrate embodiments of the application and, together with the description, serve to explain the principles of the application. In the drawings:

[0040] Figure 1 Flow chart of the multi-constraint multi-level path planning method of the embodiments of the application;

[0041] Figure 2A low-level map in a multi-level map of an embodiment of the present application;

[0042] Figure 3 A high-level map in a multi-level map of an embodiment of the present application;

[0043] Figure 4 A path planning result of a single-level map in an embodiment of the present application;

[0044] Figure 5 A low-level path in a path planning result of a multi-level map in an embodiment of the present application;

[0045] Figure 6 A high-level path in a path planning result of a multi-level map in an embodiment of the present application. DETAILED DESCRIPTION

[0046] The preferred embodiments of the present application will be described in detail with reference to the drawings, in which:

[0047] In real and virtual worlds, such as car navigation and game task guidance, we have been solving a common problem in different fields, how to reach the destination from the starting point in the shortest path. Of course, the straight line from the starting point to the destination will be the shortest distance, but this does not take into account the "passability" of the path. It is due to the limitation of passability that in the actual implementation process, it cannot completely follow the straight line to the target. The reasons for the decline or disappearance of "passability" mainly have two points, one is the obstacle to passing, and the other is the slow passing.

[0048] The obstacle to passing is an obstacle that cannot be passed directly, including natural obstacles such as cliffs, peaks, etc., and man-made obstacles such as buildings, road controls, etc. The only way to pass such obstacles is to detour, which will also increase the path cost of passing. The slow passing obstacle is an obstacle that will significantly reduce the speed of passing, including natural obstacles such as rivers, bogs, etc., and man-made obstacles such as speed limits, flow limits, etc. Such obstacles will reduce the efficiency of passing, and even change the passing method, such as changing from land to water. Especially when the cost of slow passing increases too much, slow passing can be approximately regarded as part of the obstacle to passing, because it also forms a result that must be detoured.

[0049] Cost and price are the key indicators for describing the pros and cons of a planned path in path planning, and "cost" and "price" are semantically equivalent, and are collectively referred to as price below. The lower the path price is, the shorter the total distance of the planned path is, and when assuming that the speed of completing different paths is consistent, the corresponding time of the path with the lowest price is also the shortest. Generally, the path with the shortest price is called the optimal path, and the optimal path has more time or distance advantages and should be selected by the user. Of course, the goal of the price is not necessarily to achieve the shortest path, but also to achieve the shortest time or the lowest fuel consumption. In order to be more intuitive, the length of the path is usually selected as the most basic price function target.

[0050] In the existing path determination method, only the obstacle type of hindering passing is considered, that is, when encountering an obstacle, "detour" processing is needed, and the related consideration of reducing the passing type of obstacle is lacking. When the passing type of obstacle is introduced, the cost increased by "detour" may be much higher than the cost of "crossing" the complex obstacle, such as passing through a river; at the same time, the existing path determination method is limited in two-dimensional plane, that is, the height dimension or hierarchical dimension information is not considered, such as not rising or falling in reality, or lacking Z-axis information in virtual world. The lack of dimension information will inevitably reduce the flexibility and optimality of path planning, and a new optimal path planning algorithm capable of fully utilizing the reducing type of obstacle information and hierarchical information is urgently needed.

[0051] Based on this, one specific embodiment of the present application discloses a multi-constraint multi-level path planning method, as shown in the following formula: Figure 1 The method comprises the following steps:

[0052] S1, acquiring multi-layer map information, a starting point and a target point position of path planning; each layer of map information comprises a passing type and a passing cost multiplier of each passing node in the layer of map; the passing type comprises a normal passing type, a reducing passing type and a hindering passing type;

[0053] S2, calculating an optimal path from the starting point to the target point by using an improved path planning algorithm according to the passing type and the passing cost multiplier of each passing node in each layer of map.

[0054] By taking the dimension information of Z-axis into the consideration range, and dividing the passing type into the normal passing type, the reducing passing type and the hindering passing type in parallel, the path planning is no longer only detouring, but can also consider planning the path in the Z-axis space, so that different types of obstacles can be flexibly avoided, and a more optimal path can be selected in different levels, so that the optimal path result is more accurate, and the method has stronger adaptability to the environment with obstacles and shorter path cost of planning.

[0055] For a single-level path planning task, the task plane can be abstracted into a two-dimensional plane, the two-dimensional plane is segmented, each region block is taken as a passing node, and the region block is marked as a passable region or an obstacle region according to the property of the region block. For example, a 2km*2km local area is abstracted, in order to ensure the consistency and equal proportion of the length direction and the width direction, it is assumed that 100m*100m is regionally divided, so X=20 and Y=20.

[0056] For a three-dimensional path planning task, the space needs to be segmented on the Z axis to obtain different layers, each layer can be abstracted into a two-dimensional plane, each two-dimensional plane is segmented according to a uniform segmentation scale, each region block is taken as a passing node, and the type and passing cost multiplier of the region block are marked according to the property of the region block, to obtain multi-layer map information, wherein the type of the region block includes a normal passing type, a slowed passing type and an obstructed passing type.

[0057] For example, a three-dimensional space is from the ground to 10000 meters high, and the space is 300km long and wide, the height of each layer of the map is 100m, the three-dimensional space is layered, for each layer, the region is divided according to 100m*100m, that is, each layer is rasterized, and each raster block is a passing node.

[0058] In implementation, if the path planning is an aviation path planning, geographical information and meteorological information in a three-dimensional flight space can be obtained. For example, geographical elevation data within a flyable range is extracted from a geographic information system, and meteorological cloud chart information is obtained from a meteorological satellite system or a ground observation station, the meteorological cloud chart information includes cloud type, cloud distribution range and infrared cloud chart.

[0059] Clouds are divided into three families and ten categories, as shown in Table 1.

[0060] Table 1. Cloud classification

[0061]

[0062] For each passing node, whether the passing node range contains mountains or buildings is determined according to the geographical information, if yes, the current passing node is marked as an obstructed passing type.

[0063] If the passing node range does not contain mountains and buildings, whether the passing node range contains clouds is determined according to the meteorological information, if yes, the type and passing cost multiplier of the current passing node are determined according to the type of the cloud in the current passing node range and the cloud chart information.

[0064] For example, if the cloud type belongs to the medium cloud family, the current passing node is marked as an obstructed passing type. If it does not belong to the medium cloud family, the passing type and the passing cost multiplier of the current passing node are determined according to the cloud chart information.

[0065] In implementation, the cloud image is a satellite cloud image, which is a gray image. The higher the gray value, i.e. the whiter the pixel color, the thicker the cloud or the heavier the water vapor, and the higher the cost of passing. Therefore, the type of the current passing node can be determined according to the gray value of the satellite cloud image.

[0066] For example, for a satellite cloud image with a gray value range of 0-255, if the average gray value in the range of the current passing node is in the range of 0-39, the cloud amount is very small, the passing cost is approximately the same as that in the cloud-free state, and the current passing node is marked as a normal passing type; if the average gray value is in the range of 201-255, the cloud amount is heavy, and the passing is greatly hindered, and the current passing node is marked as a hindered passing type; if the average gray value is in the range of 40-200, the current passing node is marked as a slowed passing type, and the passing cost ratio = (average gray value) / 40 + the passing cost ratio of the normal passing type.

[0067] If there is no mountain, building and cloud in the current passing node, the current passing node is marked as a normal passing type.

[0068] In the embodiments of the present application, the node type is divided according to the hindering effect of mountains, buildings and clouds on flight. In implementation, the same method can be used to divide the passing type of the passing node according to the environment of the specific planning space.

[0069] The multi-layer map space is denoted as where i represents the i-th layer map, and N represents the total number of layers. Each point in the i-th layer space corresponds to a different region block coordinate in the space, i.e. a passing node coordinate. The region block can be a passable block or an obstacle block. The L i (x, y) represents the type and passing cost ratio of the region block,

[0070]

[0071] denote that the L i (x, y) = Inf region block set is The passable region is H i = P i -B i .

[0072] For the case where the obstacle is a mountain peak, cloud and fog, the hindered passing type obstacle will change with the increase of the longitudinal height, as shown in Figure 2 , 3 is a schematic diagram of the obstacle condition of a certain low layer in the multi-layer map, Figure 2 Figure 3 ​Fig. 2 is a schematic diagram of a multi-layer map, in which the black blocks (marked with coordinate data) are the blocked nodes, such as the block (7, 9); the gray blocks with numbers are the slowed nodes, in which the numbers represent the cost multiplier, such as the node with coordinates (11, 2) has a cost multiplier of 3; the white blocks without numbers are the normal nodes, and the default cost multiplier is 1. The start point is (1, 1) and the target point is (20, 20). In order to ensure that the path does not exceed the boundary during the tracing process, the boundary is also considered as a blocked block with a cost of Inf, i.e. the gray blocks in the border area of the figure are also blocked nodes.

[0073] The multi-level path planning can reduce the total cost and flexibly avoid different types of obstacles.

[0074] After obtaining the multi-layer map information, the improved path planning algorithm is used to calculate the optimal path from the start point to the target point according to the type and cost multiplier of each node in each layer of the map in step S2.

[0075] Firstly, three sets T, V and R are defined to represent the target node set, the visited node set and the route node set respectively, and the three sets are initially set as empty sets.

[0076] The specific planning process is as follows:

[0077] S21, the start point is put into the target node set;

[0078] S22, the node with the minimum total cost value in the target node set is selected as the current parent node, and the current parent node is moved to the visited node set and the route node set;

[0079] When this step is executed for the first time, the start node is the only node in the target node set, so the start node is taken as the parent node, and the parent node is moved to the visited node set and the route node set, i.e. the parent node is deleted from the target node set and added to the visited node set and the route node set.

[0080] The path cost of the start node is 0.

[0081] If the number of nodes in the target node set is greater than 1, the nodes need to be sorted, and the node with the minimum total cost value is selected as the parent node according to the sorting result.

[0082] It should be noted that the total cost value of the node is the sum of the path cost from the start point to the parent node and the estimated cost from the current node to the target node.

[0083] Specifically, the step S22 includes:

[0084] The nodes in the set of intended nodes are sorted according to the total generation values in ascending order; if the total generation values of the nodes are the same, the nodes are sorted according to the estimated generation values from the target node in ascending order, if the estimated generation values are the same, the nodes are sorted according to the order of the nodes being added to the set of intended nodes from the rear to the front, and the intended node in the first order after the sorting is taken as the current parent node.

[0085] In order to obtain the optimal path, the nodes in the set of intended nodes are first sorted according to the total generation values in ascending order; if the total generation values of the nodes are the same, the nodes are sorted according to the estimated generation values from the target node in ascending order, if the estimated generation values are the same, in order to improve the calculation efficiency, the nodes are sorted according to the order of the nodes being added to the set of intended nodes from the rear to the front.

[0086] The intended node in the first order after the sorting is taken as the current parent node.

[0087] S23, according to the passing type and the passing cost rate of each node, the total generation value of the node in the adjacent nodes of the current parent node which is not in the set of visited nodes and is not of the obstructing passing type is calculated; the node in the adjacent nodes of the current parent node which is not in the set of visited nodes and is not of the obstructing passing type is added to the set of intended nodes;

[0088] The adjacent nodes of the current parent node include the adjacent nodes of the same layer as the current parent node and the adjacent nodes of the adjacent layer as the current parent node.

[0089] It should be noted that the mobile object is usually difficult to vertically rise or fall, therefore, in an embodiment of the present application, the adjacent nodes of the current node do not include the nodes of the adjacent layer as the current parent node which have the same x and y coordinates as the current parent node.

[0090] For example, the current parent node is the point (7, 7) in Figure 2 The adjacent nodes of the current parent node include the 8 points around the point (7, 7) in Figure 2 and the 8 points around the point (7, 7) in Figure 3

[0091] According to the passing type and the passing cost rate of each node, the total generation value of the node in the adjacent nodes of the current parent node which is not in the set of visited nodes and is not of the obstructing passing type is calculated.

[0092] For example, Figure 2 ​The intermediate nodes (6, 7) are the obstructing-passing type obstacles, so their total costs are not calculated and they are not added to the intention node set. If the node (7, 8) is already in the visited node set, its total cost is not calculated and it is not added to the intention node set.

[0093] For the adjacent nodes which are not in the visited node set and are not of the obstructing-passing type, the total cost is calculated according to the current parent node.

[0094] Firstly, the path cost is calculated according to the position relationship between the adjacent node and the parent node

[0095] Specifically, if the adjacent node and the parent node are in the same layer, the path cost is calculated by the following formula

[0096]

[0097] wherein, represents the path cost of the parent node, i represents the sequence number of the layer where the parent node and the adjacent node are located, and if the parent node is the starting node, the path cost of the parent node is 0; represents the path cost of the adjacent node; represents the cost from the parent node to the adjacent node.

[0098] That is, the path cost of the adjacent node is the sum of the path cost of the parent node and the cost from the parent node to the adjacent node.

[0099] If the adjacent node and the parent node are in different layers, the path cost is calculated by the following formula

[0100]

[0101] wherein, represents the path cost of the parent node, i represents the sequence number of the layer where the adjacent node is located, j represents the sequence number of the layer where the parent node is located, and d represents the inter-layer switching cost.

[0102] Specifically, the cost from the parent node to the adjacent node is calculated according to the formula

[0103] wherein, l represents the passing cost multiplier of the adjacent node, λ represents the cost per unit distance, and (x c , y c ) represents the planar coordinates of the adjacent node, (x f , y f ) represents the planar coordinates of the parent node.

[0104] Then, the estimated cost is calculated according to the position relationship between the adjacent node and the target node​

[0105] Specifically, if the adjacent node and the target node are in the same layer, the estimated cost is calculated by the following formula

[0106] If the adjacent node and the target node are in different layers, the estimated cost is calculated by the following formula

[0107] wherein, D m = |x c -x e | + |y c -y e |, λ represents the cost value of crossing the adjacent node, (x c , y c ) represents the plane coordinates of the adjacent node, (x e , y e ) represents the plane coordinates of the target node, d represents the inter-layer switching cost, i represents the sequence number of the layer where the adjacent node is located, j represents the sequence number of the layer where the target node is located, w1 and w2 represent weight coefficients, and

[0108] D o represents the Euclidean distance between two points, and D m represents the Manhattan distance between two points. The Manhattan distance has direction insensitivity, usually gives a higher distance value than the Euclidean distance, has two axis order opposite identical path solutions and it may not be the shortest path, and the target equidistant point is a straight line perpendicular to the line connecting the two points, resulting in slow efficiency in approaching the target. The Euclidean distance is clear in direction, and the efficiency of approaching is higher, but it is more prone to local optimal problem than the Manhattan distance, especially when there are obstacles on the path with concave direction. Taking the combination of Euclidean distance and Manhattan distance can more quickly approach the target without being trapped in local optimal for a long time. The weight of the Manhattan distance is greater than that of the Euclidean distance, which fully improves the rapidity of jumping out of the local optimal.

[0109] The total cost of the adjacent node is the sum of the path cost and the estimated cost.

[0110] After calculating the total cost of the adjacent node of the current parent node which is not in the visited node set and is not a node of the obstacle passing type, these adjacent nodes are added to the intended node set.

[0111] Therefore, the nodes in the intended node set all have the values of their total cost, path cost and estimated cost, and their corresponding parent nodes, which are convenient for subsequent determination of the path.

[0112] When a neighboring node of the current parent node is not in the visited node set and is not a node of the obstructed passage type, the node is added to the intended node set, and the following processing is performed:

[0113] For each neighboring node p i (x,y) of the current node that is not in the visited node set and is not a node of the obstructed passage type

[0114] If the node p i (x,y) is not in the intended node set, the node p i (x,y) is directly added to the intended node set, and the total cost, the path cost, the estimated cost of the node p i (x,y), and the parent node of the node p i (x,y) are recorded as the current parent node.

[0115] If the node p i (x,y) is in the intended node set, if the path cost of the node p i (x,y) calculated according to the current parent node is less than the estimated cost of the node p i (x,y) recorded in the intended node set, the parent node of the node p i (x,y) in the intended node set is updated to the current parent node, and the total cost and the path cost of the node p i (x,y) are updated to the total cost and the path cost calculated according to the current parent node.

[0116] That is, if a neighboring node of the current parent node is already in the intended node set, whether to update the information of the node in the intended node set is determined according to the total cost of the node in the intended node set and the total cost of the currently calculated neighboring node.

[0117] Specifically, if the node p i (x,y) is in the intended node set, if the total cost of the node p i (x,y) calculated according to the current parent node is less than the total cost of the node p i (x,y) recorded in the intended node set, the total cost and the path cost of the node p i (x,y) recorded in the intended node set are updated to the total cost and the path cost calculated according to the current parent node, and the parent node of the node p i (x,y) is updated to the current parent node.

[0118] S24, if a neighboring node of the current parent node contains the target node, the target node is moved to the visited node set and the planning node set, and a planning path is found according to the parent-child relationship between nodes in the planning node set.

[0119] If the target node is among the neighboring nodes of the current parent node, after moving the target node to both the visited node set and the planned node set, a path from the starting point to the target point will exist in the planned node set. Therefore, the planned path can be found based on the parent-child relationships between nodes in the planned node set.

[0120] Specifically, the planned path is found based on the parent-child relationships between nodes, including:

[0121] S241. Use the target node as the current node;

[0122] S242. Add the current node to the path node set; if the current node is the starting node, stop the search, and the sequence of nodes in the path node set is the planned path;

[0123] S243. Otherwise, find the parent node of the current node in the planning node set, and use the parent node of the previous node as the current node, then return to step S242.

[0124] That is, starting from the target node, the parent nodes of the nodes are searched in turn until the starting node is found, thus obtaining the planned path.

[0125] S25. Otherwise, return to step S22.

[0126] If the target node is not among the neighboring nodes of the current parent node, return to step S22 to continue the search.

[0127] by Figure 2 , 3 The effects of this invention will be illustrated using two layers of map information as an example. Figure 2 The map layer number is 1. Figure 3 The map layer number is 2, where the starting point is p. 1 (1,1), the target point is p 1 (20,20), the cost of crossing a unit distance λ=10, the cost of inter-layer switching d=5, w1=0.2, w2=0.8.

[0128] The planning process and effects of the multi-constraint, multi-level path planning method of the present invention will be described below using the above examples.

[0129] First, set the starting point p 1 (1,1) is added to the intended node set.

[0130] Select the node with the lowest total value from the set of potential nodes as the current parent node, i.e., p. 1 (1,1) is the current parent node, and p 1 (1,1) Move to the set of visited nodes and the set of planned nodes. At this time, the set of intended nodes is empty.

[0131] The current parent node p 1 The nodes in the adjacent nodes of (1, 1) which are not in the visited node set and are not the nodes of the obstructed passing type include p 1 (1, 2), p 1 (2, 1), p 1 (2, 2), p 2 (1, 2), p 2 (2, 1), p 2 (2, 2),

[0132] According to the formula:

[0133]

[0134] Or Or

[0135] The path cost, the estimated cost and the total cost of each adjacent node are calculated:

[0136]

[0137]

[0138]

[0139]

[0140] The p 1 (1, 2), p 1 (2, 1), p 1 (2, 2), p 2 (1, 2), p 2 (2, 1), p 2 (2, 2) are added into the intended node set.

[0141] Since the adjacent nodes of the current parent node do not include the target node, the node with the minimum total cost in the intended node set is selected as the current parent node, and since the total cost of p 1 (2, 2) is the smallest, it is selected as the current parent node, and p 1 (2, 2) is calculated. 1 (2, 2) is moved to the visited node set and the planned node set.

[0142] The current parent node p 1 The nodes in the adjacent nodes of (2, 2) which are not in the visited node set and are not the nodes of the obstructed passing type include p 1 (1, 2), p 1(1,3), p 1 (2,3), p 1 (3,3), p 1 (3,2), p 1 (3,1), p 1 (2,1), p 2 (1,1), p 2 (1,2), p 2 (1,3), p 2 (2,3), p 2 (3,3), p 2 (3,2), p 2 (3,1), p 2 (2,1). The path cost, the estimated cost and the total cost of each neighboring point are calculated according to the formula in step 23.

[0143] Since point p 1 (1,2), p 1 (2,1), p 2 (1,2), p 2 (2,1) is in the set of tentative nodes. When point p 1 (2,2) is the parent node, the path cost of point p 1 (1,2) is 24, which is greater than the path cost when point p 1 (1,1) is the parent node. Therefore, the parent node and the cost information of point p 1 (1,2) in the set of tentative nodes do not need to be updated. Similarly, the parent node and the cost information of point p 1 (2,1), p 2 (1,2), p 2 (2,1) do not need to be updated.

[0144] Point p 1 (1,3), p 1 (2,3), p 1 (3,3), p 1 (3,2), p 1 (3,1), p 2 (1,1), p 2 (1,3), p 2 (2,3), p 2 (3,3), p 2 (3,2), p 2 (3,1) are added to the set of tentative nodes.

[0145] By analogy, the search is continuously performed until the neighboring nodes of the current parent node contain the target node, i.e. the neighboring nodes contain the target point p 1(20,20), the target point is moved to the visited node set and the planning node set. In the planning node set, there is a path from the start point p 1 (1,1) to the target point p 1 (20,20).

[0146] Since the parent node information of each node is stored in the planning node set, the reverse query method can be used to query the target path.

[0147] Initially, the path node set is empty.

[0148] The point p 1 (20,20) is taken as the current node.

[0149] The current point p 1 (20,20) is added to the path node set, and the parent node of the point p 1 (20,20) is queried in the planning node set to be the point p 1 (19,19), and the point p 1 (19,19) is taken as the current point, the current point is added to the path node set, and the parent node of the current point p 1 (19,19) is queried, and so on, until the start point is found, and the start point is added to the path node set. The path in the path node set is the optimal path from the start point to the target point.

[0150] The prior art does not consider high-level information because it only plans a path on a plane, which is equivalent to only planning a path in Figure 2 According to the method of steps S21-S25 of the present application, the path planning is performed, and the path from the start point to the target point contains 27 cells (p 1 (1,1), p 1 (2,2), p 1 (3,3), p 1 (4,4), p 1 (5,5), p 1 (6,6), p 1 (7,7), p 1 (8,7), p 1 (9,6), p 1 (10,6), p 1 (11,6), p 1 (12,6), p 1 (13,6), p 1 (14,7), p 1 (15,8), p 1 (16,9), p 1 (17,10), p 1(18, 11), p 1 (19, 12), p 1 (20, 13), p 1 (20, 14), p 1 (20, 15), p 1 (20, 16), p 1 (20, 17), p 1 (20, 18), p 1 (19, 19), p 1 (20, 20), as shown in Figure 4 It should be noted that since there is only one map layer, the adjacent nodes of the current parent node in step S23 only include the same-layer nodes adjacent to the parent node.

[0151] However, when the hierarchical map is used, the path planning is performed according to the method of steps S21-S25 of the present application, and the planning result obtained is p 1 (1, 1), p 1 (2, 2), p 1 (3, 3), p 1 (4, 4), p 1 (5, 5), p 1 (6, 6), p 1 (7, 7), p 2 (8, 8), p 2 (9, 9), p 2 (9, 10), p 2 (10, 11), p 2 (11, 12), p 2 (12, 13), p 1 (13, 12), p 1 (13, 13), p 2 (14, 14), p 1 (15, 15), p 1 (16, 15), p 1 (17, 16), p 1 (18, 17), p 1 (19, 18), p 1 (19, 19), p 1 (20, 20). The planning results of the low layer and the high layer are shown in Figure 5 , Figure 6 It can be seen that the multi-layer map can obtain a shorter path planning result.

[0152] Those skilled in the art can understand that all or part of the processes of the above-mentioned embodiment methods can be completed by instructing the relevant hardware by a computer program, and the program can be stored in a computer readable storage medium. The computer readable storage medium is a disk, an optical disk, a read-only memory, a random access memory, etc.

[0153] The above description is merely preferred specific embodiments of the present application, but the protection scope of the present application is not limited thereto, and any person skilled in the art can easily think of changes or replacements within the technical scope disclosed by the present application, which should be covered within the protection scope of the present application.

Claims

1. A multi-constraint, multi-level path planning method, characterized in that, Includes the following steps: The system acquires multi-layer map information, the starting point and target point locations for path planning; each layer of map information includes the passage type and passage cost multiplier for each passage node in that layer of map; the passage type includes normal passage type, slowed passage type, and obstructed passage type; Based on the passage type and passage cost multiplier of each passage node in each layer of the map, an improved path planning algorithm is used to calculate the optimal path from the starting point to the target point. Based on the access type and access cost multiplier of each access node in each map layer, an improved path planning algorithm is used to calculate the optimal path from the starting point to the target point, including: S21. Add the starting point to the intended node set; S22. Select the node with the lowest total value from the set of intended nodes as the current parent node, and move the current parent node to the set of visited nodes and the set of planned nodes. S23. Based on the passage type and passage cost multiplier of each node, calculate the total cost of nodes that are not in the visited node set and are not obstructing passage among the neighboring nodes of the current parent node; add the nodes that are not in the visited node set and are not obstructing passage among the neighboring nodes of the current parent node to the intention node set; the total cost of the node is the sum of the path cost from the starting point through the parent node to the current node and the estimated cost from the current node to the target node; S24. If the target node is among the neighboring nodes of the current parent node, then move the target node to the set of visited nodes and the set of planned nodes; in the set of planned nodes, find the planned path based on the parent-child relationship between the nodes. S25. Otherwise, return to step S22.

2. The multi-constraint, multi-level path planning method according to claim 1, characterized in that, For each neighboring node p of the current node that is not in the set of visited nodes and is not an obstruction to passage i (x,y): If node p i If (x, y) is not in the intended node set, then directly add node p. i (x, y) is added to the intention node set, and node p is recorded. i The total cost, path cost, estimated cost, and p of (x,y) i The parent node of (x,y) is the current parent node; If node p i If (x, y) is already in the target node set, then if node p is calculated based on the current parent node... i The path cost of (x, y) is less than the cost of node p recorded in the intention node set. i The estimated cost of (x,y) will then be the node p in the intention node set. i The parent node of (x, y) updates the current parent node, and sets p i The total cost and path cost of (x,y) are updated to the total cost and path cost calculated based on the current parent node, respectively.

3. The multi-constraint, multi-level path planning method according to claim 1, characterized in that, Select the node with the lowest total value from the set of intended nodes as the current parent node, including: Sort the nodes in the intended node set according to the total generation value from smallest to largest; if the total generation value of the nodes is the same, sort them according to the estimated generation value from the node to the target node from smallest to largest; if the estimated generation value is the same, sort them according to the order in which the nodes were added to the intended node set from last to first, and take the intended node in the first position after sorting as the current parent node.

4. The multi-constraint, multi-level path planning method according to claim 1, characterized in that, The neighboring nodes of the current parent node include nodes at the same level as the current parent node, as well as nodes at an adjacent level.

5. The multi-constraint, multi-level path planning method according to claim 4, characterized in that, If the neighboring node and the parent node are on the same level, the path cost is calculated using the following formula. If neighboring nodes and parent nodes are on different layers, the path cost is calculated using the following formula. in, This represents the path cost of the parent node located at level i. This represents the path cost of the parent node located at level j. If the parent node is the starting node, then the path cost of the parent node is 0. Let represent the path cost to neighboring nodes in layer i, and d represent the inter-layer switching cost. This represents the cost from the parent node to the neighboring node.

6. The multi-constraint, multi-level path planning method according to claim 5, characterized in that, According to the formula Calculate the cost from the parent node to neighboring nodes. Where l represents the cost multiplier of neighboring nodes, λ represents the cost per unit distance traveled, (x c ,y c (x) represents the planar coordinates of neighboring nodes. f ,y f ) represents the planar coordinates of the parent node.

7. The multi-constraint, multi-level path planning method according to claim 4, characterized in that, When neighboring nodes and the target node are on the same layer, the estimated cost is calculated using the following formula. in, D m =|x c -x e |+|y c -y e |,λ represents the cost per unit distance traveled, (x c ,y c (x) represents the planar coordinates of neighboring nodes. e ,y e ) represents the planar coordinates of the target node, and w1 and w2 represent the weight coefficients.

8. The multi-constraint, multi-level path planning method according to claim 4, characterized in that, When neighboring nodes and the target node are on different layers, the estimated cost is calculated using the following formula. in, D m =|x c -x e |+|y c -y e |,λ represents the cost per unit distance traveled, (x c ,y c (x) represents the planar coordinates of neighboring nodes. e ,y e ) represents the planar coordinates of the target node, w1 and w2 represent the weight coefficients, d represents the inter-layer switching cost, i represents the index of the layer where the neighboring node is located, and j represents the index of the layer where the target node is located.

9. The multi-constraint, multi-level path planning method according to claim 1, characterized in that, The planned path is obtained from the set of planning nodes based on the parent-child relationships between nodes, including: S241. Use the target node as the current node; S242. Add the current node to the path node set; if the current node is the starting node, stop the search, and the sequence of nodes in the path node set is the planned path; S243. Otherwise, find the parent node of the current node in the planning node set, take the parent node of the current node as the current node, and return to step S242.

Images