Path Planning Method and Device

Abstract

The present invention relates to the field of computers, and provides a path planning method and apparatus. The method includes: determining a starting point and a target point of a path to be planned for a target object; determining the path to be planned based on the starting point and the target point according to the hybrid A* algorithm; in the hybrid A* algorithm, the cost function of an expanded node in the open list corresponding to the current node includes a turning cost; the turning cost is obtained based on the difference between the turning direction flag bit of the current node and the turning direction flag bit of the expanded node. In this way, the problem of the turning direction is considered, making the planned path smoother, avoiding the occurrence of unstable phenomena such as continuous turning during driving, thereby improving problems such as sudden speed changes and poor driving stability. Moreover, the turning cost is obtained through the difference between the turning direction flag bit of the current node and the turning direction flag bit of the expanded node, with higher calculation efficiency, thus improving the efficiency of path planning and quickly obtaining a smooth planned path.

Description

Technical Field

[0001] The present invention relates to the field of computer technology, and particularly to a path planning method and device. Background Art

[0002] Path planning is to search for a path from a starting point to a target point for a target object. The traditional A* (A-Star) algorithm is a commonly used path planning algorithm. However, the traditional A* algorithm does not consider the kinematic constraints of the target object, and the planned path may have a problem of too large turning amplitude, resulting in a deviation between the actual movement path of the target object and the planned path. Although the path planning generated by the hybrid A* algorithm takes into account the kinematic constraints, there is a problem of continuous turning in the planned path, which easily leads to problems such as sudden speed change and poor driving stability. Summary of the Invention

[0003] The present invention provides a path planning method and device to solve the problem of continuous turning in the path planned by the hybrid A* in the prior art, which easily leads to problems such as sudden speed change and poor driving stability, and realizes quickly obtaining a smooth planned path.

[0004] The present invention provides a path planning method, including:

[0005] Determine the starting point and the target point of the path to be planned for the target object;

[0006] Based on the starting point and the target point, determine the path to be planned according to the hybrid A* algorithm;

[0007] In the hybrid A* algorithm, the cost function of the expanded node in the open list corresponding to the current node includes a turning cost; the turning cost is obtained based on the difference between the turning direction flag bit of the current node and the turning direction flag bit of the expanded node.

[0008] According to the path planning method provided by the present invention, the turning cost is the product of the absolute value of the difference and a preset coefficient.

[0009] According to the path planning method provided by the present invention, the turning direction flag bit is a first flag bit for representing a left turn, a second flag bit for representing a straight line, or a third flag bit for representing a right turn.

[0010] According to the path planning method provided by the present invention, the cost function is the sum of the path cost between the current node and the starting point, the path cost between the current node and the expanded node, the path cost between the expanded node and the target point, and the turning cost.

[0011] A path planning method provided by the present invention, determining the to-be-planned path according to the hybrid A* algorithm, includes:

[0012] Perform the following processing steps on the current node:

[0013] Generate the shortest path between the current node and the target point based on the kinematic constraints of the target object;

[0014] Detect whether the target object collides with each target obstacle under the shortest path;

[0015] If a collision occurs, generate the expansion nodes with different turning directions corresponding to the current node. If it is determined that the expansion node is not an obstacle, add the expansion node to the open list, select the expansion node with the minimum cost function in the open list as the new current node, and execute the processing steps;

[0016] If no collision occurs, determine the to-be-planned path based on the shortest path.

[0017] A path planning method provided by the present invention, the detecting whether the target object collides with each target obstacle under the shortest path includes:

[0018] For each path point in the shortest path, perform the following collision detection steps:

[0019] Generate a first geometric figure enclosing the target object based on the size information of the target object, and generate a second geometric figure enclosing the target obstacle based on the size information of the target obstacle;

[0020] In the world coordinate system, determine the coordinates of the target object based on the coordinates of the path point, solve the expression equation of the first geometric figure based on the coordinates of the target object and the size information of the target object, and solve the expression equation of the second geometric figure based on the coordinates of the target obstacle and the size information of the target obstacle;

[0021] If it is determined that there is an intersection point belonging to the area enclosed by the second geometric figure between the expression equation of the first geometric figure and the expression equation of the second geometric figure, determine that the target object collides with the target obstacle, otherwise, determine that the target object does not collide with the target obstacle.

[0022] A path planning method provided by the present invention, both the first geometric figure and the second geometric figure are polygons, and the expression equations of the first geometric figure and the second geometric figure are the expression equations of the straight lines where the sides of the polygon are located.

[0023] According to a path planning method provided by the present invention, both the first geometric figure and the second geometric figure are rectangles, and the expression equations of the first geometric figure and the expression equations of the second geometric figure are the expression equations of the four straight lines where the sides of the rectangle are located.

[0024] According to a path planning method provided by the present invention, before adding the expanded node to the open list if it is determined that the expanded node is not an obstacle, it further includes:

[0025] Based on the grid accuracy of the grid coordinate system and the size information of the target object, determine the safety distance in the grid coordinate system. In the grid coordinate system, expand with the coordinates of the target obstacle as the center and the safety distance as the radius, and determine the grid within the expanded area as the area of the virtual obstacle;

[0026] If the expanded node is outside the area of the virtual obstacle, determine that the expanded node is not an obstacle.

[0027] The present invention also provides a path planning device, including:

[0028] A first determination module, configured to determine the starting point and the target point of the path to be planned for the target object;

[0029] A second determination module, configured to determine the path to be planned based on the starting point and the target point according to the hybrid A* algorithm;

[0030] In the hybrid A* algorithm, the cost function of the expanded node in the open list corresponding to the current node includes a turning cost; the turning cost is obtained based on the difference between the turning direction flag bit of the current node and the turning direction flag bit of the expanded node.

[0031] According to a path planning device provided by the present invention, the second determination module is specifically configured to:

[0032] Perform the following processing steps on the current node:

[0033] Based on the kinematic constraints of the target object, generate the shortest path between the current node and the target point;

[0034] Detect whether the target object collides with each target obstacle under the shortest path;

[0035] If a collision occurs, generate the expansion nodes corresponding to different turning directions of the current node. If it is determined that the expansion node is not an obstacle, add the expansion node to the open list, select the expansion node with the minimum cost function in the open list as the new current node, and execute the processing step;

[0036] If no collision occurs, determine the path to be planned based on the shortest path.

[0037] According to a path planning device provided by the present invention, the second determination module is specifically configured to:

[0038] For each path point in the shortest path, execute the following collision detection steps:

[0039] Generate a first geometric figure that encloses the target object based on the size information of the target object, and generate a second geometric figure that encloses the target obstacle based on the size information of the target obstacle;

[0040] In the world coordinate system, determine the coordinates of the target object based on the coordinates of the path point, solve the expression equation of the first geometric figure based on the coordinates of the target object and the size information of the target object, and solve the expression equation of the second geometric figure based on the coordinates of the target obstacle and the size information of the target obstacle;

[0041] If it is determined that there is an intersection point belonging to the area enclosed by the second geometric figure between the expression equation of the first geometric figure and the expression equation of the second geometric figure, determine that the target object collides with the target obstacle, otherwise, determine that the target object does not collide with the target obstacle.

[0042] According to a path planning device provided by the present invention, the second determination module is further configured to:

[0043] Based on the grid accuracy of the grid coordinate system and the size information of the target object, determine the safety distance in the grid coordinate system. In the grid coordinate system, expand with the coordinates of the target obstacle as the center and the safety distance as the radius, and determine the grid in the expanded area as the area of the virtual obstacle;

[0044] If the expansion node is outside the area of the virtual obstacle, determine that the expansion node is not an obstacle.

[0045] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, it implements the path planning method as described in any one of the above.

[0046] The present invention also provides a non-transitory computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the path planning method as described in any one of the above is implemented.

[0047] The present invention also provides a computer program product, including a computer program, and when the computer program is executed by a processor, the path planning method as described in any one of the above is implemented.

[0048] The path planning method provided by the present invention improves the hybrid A* algorithm. The cost function of the hybrid A* algorithm includes a turning cost, and the turning cost is obtained based on the difference between the turning direction flag bit of the current node and the turning direction flag bit of the expanded node. In this way, the problem of the turning direction is considered, making the planned path smoother, avoiding the occurrence of unstable phenomena of continuous turning during driving, thereby improving problems such as sudden speed change and poor driving stability. Moreover, the turning cost is obtained through the difference between the turning direction flag bit of the current node and the turning direction flag bit of the expanded node, with higher calculation efficiency, thus improving the efficiency of path planning and quickly obtaining a smooth planned path. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following will briefly introduce the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings in the following description are some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0050] Figure 1 is a schematic flowchart of the path planning method provided by the present invention;

[0051] Figure 2 is a schematic diagram of the turning direction flag bit provided by the present invention;

[0052] Figure 3 is a schematic diagram of the coordinates of the current node and the expanded node provided by the present invention;

[0053] Figure 4 is a schematic diagram of the to-be-planned path obtained by the present invention;

[0054] Figure 5 is a schematic diagram of the collision detection scenario provided by the present invention;

[0055] Figure 6 is a schematic diagram of the structure of the path planning device provided by the present invention;

[0056] Figure 7 is a schematic diagram of the structure of the electronic device provided by the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0057] To make the objectives, technical solutions and advantages of the present invention clearer, the technical solutions in the present invention will be clearly and completely described below with reference to the accompanying drawings in the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in the present invention without creative efforts shall fall within the protection scope of the present invention.

[0058] The following Figures 1 to 7 describes the path planning method and device of the present invention.

[0059] An embodiment of the present invention provides a path planning method for performing path planning for a target object, where the target object is the object that needs to perform path planning, and can be an unmanned device, such as an unmanned excavator, a robot, etc. This path planning method can be applied to the target object and executed by the target object or the software and / or hardware therein, or can be applied to a server and executed by the server or the software and / or hardware therein. Based on this, the server can send the obtained planned path to the target object. The path planning method provided by the embodiment of the present invention will be introduced in detail below.

[0060] Figure 1 is a schematic flowchart of the path planning method provided by the present invention.

[0061] As Figure 1 shown, this embodiment provides a path planning method, which at least includes:

[0062] Step 101: Determine the starting point and the target point of the path to be planned for the target object.

[0063] The target point here is the destination that the target object needs to reach. In this step, the coordinates of the starting point and the target point can be specifically determined.

[0064] Step 102: Based on the starting point and the target point, determine the path to be planned according to the hybrid A* algorithm; in the hybrid A* algorithm, the cost function of the expanded node in the open list corresponding to the current node includes the turning cost; the turning cost is obtained based on the difference between the turning direction flag bit of the current node and the turning direction flag bit of the expanded node.

[0065] The hybrid A* algorithm takes the starting point as the current node, creates an Open list and a Close list, expands the current node to obtain expanded nodes, adds the expanded nodes to the Open list, selects the expanded node with the minimum cost function in the Open list as the new current node (i.e., the principle of the minimum cost value) and adds it to the Close list, and deletes the expanded node with the minimum cost function in the Open list. Thus, continue to expand the new current node. When the distance between the selected expanded node with the minimum cost function and the target point is less than or equal to the preset distance, stop expanding the node. Among them, the preset distance can be set according to the actual situation and is not limited here.

[0066] When expanding the current node, node expansion in different turning directions can be performed to obtain expanded nodes in different turning directions.

[0067] In this embodiment, the hybrid A* algorithm is improved. The cost function of the hybrid A* algorithm includes a turning cost, which is obtained based on the difference between the turning direction flag bit of the current node and the turning direction flag bit of the expanded node. In this way, the problem of the turning direction is considered, making the planned path smoother, avoiding the appearance of unstable phenomena such as continuous turning during driving, thereby improving problems such as sudden speed change and poor driving stability. And the turning cost is obtained through the difference between the turning direction flag bit of the current node and the turning direction flag bit of the expanded node, with higher calculation efficiency, thus improving the efficiency of path planning and quickly obtaining a smooth planned path.

[0068] Taking an unmanned excavator as an example, in post-disaster areas or environments such as toxic and harmful areas, the unmanned excavator can achieve continuous autonomous operation on the operation object, can adapt to a variety of different working objects, and with the iterative upgrade of the performance of the unmanned excavator, it can gradually replace skilled operators, making the unmanned excavator have the advantages of autonomous operation and low labor cost, and becoming one of the research hotspots in recent years. Path planning is an important link for the automatic walking of an unmanned excavator. The path planned by the hybrid A* algorithm in the prior art has the problem of continuous turning, which is likely to cause problems such as sudden speed change and poor driving stability. However, by using the improved hybrid A* algorithm provided in the embodiment of the present invention, these problems can be effectively improved.

[0069] Based on the above embodiments, the turning cost is the product of the absolute value of the difference and a preset coefficient.

[0070] Exemplarily, when expanding the current node, expanded nodes in three turning directions, namely a left-turn node, a straight-ahead node, and a right-turn node, can be obtained. Correspondingly, the above-mentioned turning direction flag bits are a first flag bit for representing a left turn, a second flag bit for representing a straight-ahead, or a third flag bit for representing a right turn.

[0071] The steering direction flag bits can be set according to the actual situation. Exemplarily, the first flag bit is -1, the second flag bit is 0, and the third flag bit is 1. As Figure 2 shown, starting from the starting point as the current node, extended nodes with three steering directions of -1, 0, and 1 are obtained. When an extended node is selected as the current node, extended nodes with three steering directions of -1, 0, and 1 will continue to be obtained.

[0072] The steering direction flag bit of the current node is denoted as t i-1 , and the steering direction flag bit of the extended node is denoted as t i , then the steering cost Δt i is:

[0073] Δt i = K · |t i-1 - t i | (1)

[0074] where K is a preset coefficient.

[0075] If the first flag bit is -1, the second flag bit is 0, and the third flag bit is 1, the value of Δt i can be 0, K, 2K.

[0076] Based on this, the cost function is the sum of the path cost between the current node and the starting point, the path cost between the current node and the extended node, the path cost between the extended node and the target point, and the steering cost. The specific formula for the cost function F is as follows:

[0077] F = g i-1 + l i + H i + Δt i (2)

[0078] where: g i-l is the path cost between the current node and the starting point; l i is the path cost between the extended node and the current node; H i is the path cost between the extended node and the target point.

[0079] The path from the starting point to the current node is the planned path, and the path cost between the current node and the starting point is the length of the path from the starting point to the current node.

[0080] The path cost between the extended node and the current node is the length of the path between the extended node and the current node.

[0081] The path cost between the expanded node and the target point is the length of the estimated path between the expanded node and the target point. The length of the estimated path between the expanded node and the target point can be obtained in the following ways: Way 1: Based on kinematic constraints, the shortest path between the expanded node and the target point can be generated, and the length of this shortest path can be obtained as the length of the estimated path between the expanded node and the target point. Exemplarily, the Dubins curve can be used to generate this shortest path. Way 2: The Euclidean distance between the expanded node and the target point can be calculated as the length of the estimated path between the expanded node and the target point. Way 3: The maximum value of the two in Way 1 and Way 2 can be selected as the length of the estimated path between the expanded node and the target point.

[0082] Based on the above embodiments, according to the hybrid A* algorithm, the path to be planned is determined, and its specific implementation manner may include: performing the following processing steps on the current node:

[0083] The first step: Based on the kinematic constraints of the target object, generate the shortest path between the current node and the target point.

[0084] In this step, the shortest path between the current node and the target node can be generated based on the Dubins curve or the RS curve. RS is the abbreviation of Reeds-Shepp. Both the Dubins curve and the RS curve are existing ways to generate paths, which will not be elaborated here.

[0085] The second step: Detect whether the target object collides with each target obstacle under the shortest path.

[0086] The target obstacles here are the obstacles in the area including the starting point and the target point. In practical applications, the obstacle detection device set in the target object can be used to detect the obstacles in this area. Exemplarily, the lidar is used to collect the laser point cloud data of this area, and the information of the target obstacles, such as the laser point cloud coordinates, is extracted, so as to complete the detection of the target obstacles. Based on this, detect whether the target object collides with each target obstacle under the shortest path generated in the first step.

[0087] The third step: If a collision occurs, generate expanded nodes with different steering directions corresponding to the current node. If it is determined that the expanded node is not an obstacle, add the expanded node to the open list, select the expanded node with the minimum cost function in the open list as the new current node, and execute the above processing steps.

[0088] In this step, if a collision occurs, the shortest path generated in the first step is discarded, and the node expansion continues to generate expanded nodes with different steering directions corresponding to the current node. As Figure 3As shown, when generating the extended nodes corresponding to different turning directions of the current node, the coordinates of the extended nodes can be calculated in the following specific manner:

[0089] First, obtain the minimum turning radius R of the target object 0 and the extension step length l.

[0090] Then, based on the minimum turning radius R 0 and the extension step length l, determine the change value of the heading angle The heading angle is the angle between the direction of the centroid velocity of the target object and the horizontal axis. In the world coordinate system (composed of three mutually perpendicular and intersecting coordinate axes X, Y, and Z), the horizontal axis is the X-axis. Specifically, the change value of the heading angle is determined according to the following formula

[0091]

[0092] Subsequently, denote the current node as As Figure 3 shown, determine the coordinates of the left-turn node n L according to the following formula the coordinates of the straight-ahead node n S and the coordinates of the right-turn node n R

[0093]

[0094]

[0095]

[0096] It should be noted that if the distance between the extended node with the minimum cost function in the selected open list and the target point is less than or equal to the preset distance, then stop expanding the node.

[0096]

[0097] Step 4: If no collision occurs, based on the shortest path in Step 1, determine the path to be planned.

[0098] Specifically, if the current node is the starting point and the shortest path from the starting point to the target point generated in Step 1 does not collide, then directly determine this shortest path as the path to be planned. If the current node is not the starting point, as Figure 4 shown, then combine the planned path between the starting point and the current node and the shortest path in Step 1 to obtain the path to be planned. In this way, as long as the shortest path in Step 1 does not collide with the target obstacle, there is no need to expand the node any further.

[0099] In this embodiment, before each node expansion, the shortest path from the current node to the target point is first generated, and then collision detection is performed. If no collision occurs, node expansion is no longer performed, and the path search is completed, thereby improving the efficiency of path planning. In addition, kinematic constraints are considered in path planning to avoid the problem of too large turning angles in the planned path, so that the actual driving path is consistent with the planned path.

[0100] Based on the above embodiments, to detect whether the target object collides with each target obstacle under the shortest path, the specific implementation method may include: for each path point in the shortest path, perform the following collision detection steps:

[0101] First step, based on the size information of the target object, generate a first geometric figure that encloses the target object, and based on the size information of the target obstacle, generate a second geometric figure that encloses the target obstacle.

[0102] Among them, the first geometric figure and the second geometric figure have the same shape, and their specific shape can be set according to actual needs. Exemplarily, both the first geometric figure and the second geometric figure are polygons (such as rectangles) or circles.

[0103] In practical applications, the size information of the target obstacle can be detected by an obstacle detection device. For example, in the world coordinate system, the first distance between the maximum coordinate and the minimum coordinate of the X axis and the second distance between the maximum coordinate and the minimum coordinate of the Y axis can be determined using the laser point cloud coordinates of the target obstacle as the size information of the target obstacle.

[0104] Taking an unmanned excavator as an example, as Figure 5 shown, the size information of the target object includes the width a of the lower body of the excavator, the length L from the center of the rear axle to the front end of the excavator 1 , and the length L from the center of the rear axle to the rearmost end of the excavator 2 .

[0105] Second step, in the world coordinate system, determine the coordinates of the target object based on the coordinates of the path point, solve the expression equation of the first geometric figure based on the coordinates of the target object and the size information of the target object, and solve the expression equation of the second geometric figure based on the coordinates of the target obstacle and the size information of the target obstacle.

[0106] Here, the coordinates of the target object can be the centroid coordinates of the target object. The coordinates of the target obstacle can be obtained based on the laser point cloud coordinates, for example, they can be the center coordinates of the target obstacle.

[0107] If both the first geometric figure and the second geometric figure are polygons, the expression equations of the first geometric figure and the second geometric figure are the expression equations of the straight lines where the sides of the polygon are located. The following takes the polygon as a rectangle as an example for illustration:

[0108] If both the first geometric figure and the second geometric figure are rectangles, the generation of the expression equations is relatively simple. The expression equations of the first geometric figure and the second geometric figure are the expression equations of the four straight lines where the sides of the rectangle are located. The path points on the Dubins curve are the coordinates of the center of the rear axle (the center of mass) and the heading angle of the excavator in the world coordinate system, which are Then, as Figure 5 shown, in the world coordinate system, the coordinates of the four corner points of the first geometric figure can be determined according to the following formula, that is, the coordinates of point A (x A , y A ), the coordinates of point B (x B , y B ), the coordinates of point C (x C , y C ), and the coordinates of point D (x D , y D ):

[0109]

[0110]

[0111]

[0112]

[0113] Among them: L a is the length of the line segment from the center of mass to point A, and L b is the length of the line segment from the center of mass to point D. α 1 is the angle between L a and the direction of the center of mass velocity. α 2 is the angle between L b and the direction opposite to the direction of the center of mass velocity.

[0114]

[0115] Finally, based on the coordinates of the four corner points of the first geometric figure, four straight line equations L AB , L BC , L CD , and L DA can be determined.

[0116] Among them, L AB is the expression equation of the straight line where points A and B are located, and L BC is the expression equation of the straight line where points B and C are located, and LCD is the expression equation of the straight line where points C and D are located, L DA is the expression equation of the straight line where points D and A are located.

[0117] For the specific method of determining the expression equation of a straight line based on the coordinates of two points, reference can be made to the implementation of related technologies, which will not be elaborated here.

[0118] For the second geometric figure, in the size information of the target obstacle, the first distance is denoted as L 3 , and the second distance is denoted as L 4 , the center coordinates of the target obstacle are denoted as (x s′ , y s′ ), and the four corner points of the second geometric figure are E, J, G, and H respectively. Then, four straight line equations L EJ , L JG , L GH and L HE can be determined. L EJ is: y s′ +L 4 / 2. L JG is: x s′ +L 3 / 2. L GH is: y s′ -L 4 / 2. L HE is: x s′ -L 3 / 2.

[0119] Step 3: If it is determined that there are intersections belonging to the area enclosed by the second geometric figure in the expression equations of the first geometric figure and the second geometric figure, it is determined that the target object collides with the target obstacle; otherwise, it is determined that the target object does not collide with the target obstacle.

[0120] Still taking the first geometric figure and the second geometric figure both being rectangles as an example, determine whether the four straight line equations of the first geometric figure have intersections with the four straight lines of the second geometric figure. If there are no intersections, no collision occurs; if there are intersections and the intersection coordinates are within the area enclosed by the second geometric figure, a collision occurs; if the intersection coordinates are outside the area enclosed by the second geometric figure, no collision occurs.

[0121] Figure 5 In the path shown by the curve in , the rectangle enclosing the target object has two intersections P1 and P2 with the rectangle enclosing the target obstacle in the lower right corner, and a collision will occur, and there is no intersection with the rectangle enclosing the target obstacle in the upper right corner, so no collision occurs.

[0122] In this embodiment, collision detection is performed in the world coordinate system, which solves the problem of discretization in the grid coordinate system. At the same time, based on the collision detection of the geometric figures enclosing the target obstacle and the target object intersecting, it can effectively avoid the occurrence of missed collision problems caused by the generated path passing through the middle of two obstacles, improving the accuracy of collision detection.

[0123] Based on the above embodiments, before adding the expansion node to the open list if it is determined that the expansion node is not an obstacle, the above method may further include:

[0124] First step: Based on the grid accuracy of the grid coordinate system and the size information of the target object, determine the safety distance in the grid coordinate system. In the grid coordinate system, with the coordinates of the target obstacle as the center and the safety distance as the radius, perform inflation, and determine the grid within the inflated area as the area of the virtual obstacle.

[0125] In practical applications, the map coordinates of the area including the starting point and the target point in the world coordinate system can be pre-converted to the grid coordinate system, that is, rasterize the map. The coordinate conversion formula is as follows:

[0126]

[0127] Among them, (x, y) are the coordinates in the world coordinate system, min x is the minimum value of the map coordinates on the X-axis, min y is the minimum value of the map coordinates on the Y-axis, (x s , y s ) are the coordinates in the grid coordinate system, gres is the preset grid accuracy of the grid coordinate system, the unit can be meters / pixel, and ceil is the rounding function.

[0128] Exemplarily, the calculation formula for the safety distance d is as follows:

[0129] d = ceil(r n / gres) × 2 + 1 (13)

[0130] r n = (L 1 + L 2 ) / 2 (14)

[0131] Among them, rn is half of the maximum size of the target object.

[0132] In implementation, the coordinates of the target obstacle and the expansion node in the world coordinate system can be converted to the grid coordinate system.

[0133] Second step: If the expansion node is outside the area of the virtual obstacle, determine that the expansion node is not an obstacle.

[0134] Correspondingly, if the expanded node is in the area of the virtual obstacle, it indicates that the expanded node is an obstacle, and this expanded node is discarded.

[0135] In the prior art, when using the hybrid A* algorithm for path planning, the target object is simplified to a particle. If the size of the target object itself is relatively large, when the planned path is close to the obstacle, it is easy to collide with the obstacle. In this embodiment, the size of the target object is considered during path planning, and the obstacle is inflated, so that the finally generated path always maintains a certain safety distance from the obstacle, ensuring the collision-free movement of the large-sized target object. In addition, only the obstacle is virtually inflated without changing the current map, ensuring that the original map information remains unchanged, facilitating the calculation of the positional relationship between the obstacle and the target object, and improving the search efficiency.

[0136] The solution of the embodiment of the present invention improves the problems that the existing hybrid A* algorithm path planning does not consider the size, continuous turning, etc. of the moving body (i.e., the target object). It can use the Dubins curve to generate the path from the current node to the target point. If this path meets the collision-free requirement, the path search is completed. If this path collides with the obstacle, the current node uses the improved hybrid A* algorithm to complete node expansion, converts the expanded node to the grid coordinate system, and determines whether the expanded node is an obstacle. If it is an obstacle, the expanded node is discarded; if it is not an obstacle, it is retained. The expanded node is selected according to the principle of the minimum cost value, and the selected expanded node is used as the current node. Then, the Dubins curve is used again to generate the path and perform collision detection. The above steps are repeated until the path search is completed. Moreover, a collision detection method for straight line intersection is proposed to discard the Dubins curve path with collision; a safety distance is also introduced to ensure that there is a certain safety distance between the generated path and the obstacle.

[0137] The path planning device provided by the present invention is described below. The path planning device described below can be correspondingly referred to the path planning method described above.

[0138] Figure 6 is a schematic structural diagram of the path planning device provided by the present invention.

[0139] As Figure 6 shown, this embodiment provides a path planning device, including:

[0140] A first determination module 601, configured to determine the starting point and the target point of the path to be planned for the target object;

[0141] A second determination module 602, configured to determine the path to be planned based on the starting point and the target point according to the hybrid A* algorithm;

[0142] In the hybrid A* algorithm, the cost function of the expanded node in the open list corresponding to the current node includes the turning cost; the turning cost is obtained based on the difference between the turning direction flag bit of the current node and the turning direction flag bit of the expanded node.

[0143] Based on the above embodiments, the turning cost is the product of the absolute value of the difference and a preset coefficient.

[0144] Based on the above embodiments, the turning direction flag bit is the first flag bit for characterizing a left turn, the second flag bit for characterizing a straight line, or the third flag bit for characterizing a right turn.

[0145] Based on the above embodiments, the cost function is the sum of the path cost between the current node and the starting point, the path cost between the current node and the expanded node, the path cost between the expanded node and the target point, and the turning cost.

[0146] Based on the above embodiments, the second determination module is specifically configured to:

[0147] Perform the following processing steps on the current node:

[0148] Generate the shortest path between the current node and the target point based on the kinematic constraints of the target object;

[0149] Detect whether the target object collides with each target obstacle under the shortest path;

[0150] If a collision occurs, generate expanded nodes with different turning directions corresponding to the current node. If it is determined that the expanded node is not an obstacle, add the expanded node to the open list, select the expanded node with the minimum cost function in the open list as the new current node, and execute the processing steps;

[0151] If no collision occurs, determine the path to be planned based on the shortest path.

[0152] Based on the above embodiments, the second determination module is specifically configured to:

[0153] For each path point in the shortest path, perform the following collision detection steps:

[0154] Generate a first geometric figure that encloses the target object based on the size information of the target object, and generate a second geometric figure that encloses the target obstacle based on the size information of the target obstacle;

[0155] In the world coordinate system, determine the coordinates of the target object based on the coordinates of the path point, solve the expression equation of the first geometric figure based on the coordinates of the target object and the size information of the target object, and solve the expression equation of the second geometric figure based on the coordinates of the target obstacle and the size information of the target obstacle;

[0156] If it is determined that there is an intersection point within the area enclosed by the second geometric figure in the expression equations of the first geometric figure and the second geometric figure, it is determined that the target object collides with the target obstacle; otherwise, it is determined that the target object does not collide with the target obstacle.

[0157] Based on the above embodiments, both the first geometric figure and the second geometric figure are polygons, and the expression equations of the first geometric figure and the second geometric figure are the expression equations of the straight lines where the sides of the polygons are located.

[0158] Based on the above embodiments, both the first geometric figure and the second geometric figure are rectangles, and the expression equations of the first geometric figure and the second geometric figure are the expression equations of the four straight lines where the sides of the rectangles are located.

[0159] Based on the above embodiments, the second determination module is further configured to:

[0160] Based on the grid accuracy of the grid coordinate system and the size information of the target object, determine the safety distance in the grid coordinate system. In the grid coordinate system, take the coordinates of the target obstacle as the center and the safety distance as the radius for dilation, and determine the area of the grid within the dilated area as the area of the virtual obstacle;

[0161] If the expansion node is outside the area of the virtual obstacle, determine that the expansion node is not an obstacle.

[0162] Figure 7 Illustrates a schematic physical structure diagram of an electronic device, as Figure 7 shown. The electronic device may include: a processor 710, a communication interface 720, a memory 730, and a communication bus 740. Among them, the processor 710, the communication interface 720, and the memory 730 complete mutual communication through the communication bus 740. The processor 710 may call the logical instructions in the memory 730 to execute a path planning method, and the method includes:

[0163] Determine the starting point and the target point of the path to be planned for the target object;

[0164] Based on the starting point and the target point, determine the path to be planned according to the hybrid A* algorithm;

[0165] In the hybrid A* algorithm, the cost function of the expansion node in the open list corresponding to the current node includes a turning cost; the turning cost is obtained based on the difference between the turning direction flag bit of the current node and the turning direction flag bit of the expansion node.

[0166] In addition, when the logical instructions in the above-mentioned memory 730 are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present invention. The aforementioned storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memories (ROMs), random access memories (RAMs), magnetic disks, or optical discs that can store program codes.

[0167] On the other hand, the present invention also provides a computer program product. The computer program product includes a computer program stored on a non-transitory computer-readable storage medium. The computer program includes program instructions. When the program instructions are executed by a computer, the computer can execute the path planning method provided by the above-mentioned various methods. The method includes:

[0168] Determine the starting point and the target point of the path to be planned for the target object;

[0169] Based on the starting point and the target point, determine the path to be planned according to the hybrid A* algorithm;

[0170] In the hybrid A* algorithm, the cost function of the expanded node in the open list corresponding to the current node includes a turning cost; the turning cost is obtained based on the difference between the turning direction flag bit of the current node and the turning direction flag bit of the expanded node.

[0171] On yet another aspect, the present invention also provides a non-transitory computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, it is implemented to execute the path planning method provided by the above-mentioned various methods. The method includes:

[0172] Determine the starting point and the target point of the path to be planned for the target object;

[0173] Based on the starting point and the target point, determine the path to be planned according to the hybrid A* algorithm;

[0174] In the hybrid A* algorithm, the cost function of the expanded node in the open list corresponding to the current node includes a turning cost; the turning cost is obtained based on the difference between the turning direction flag bit of the current node and the turning direction flag bit of the expanded node.

[0175] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. Those of ordinary skill in the art can understand and implement it without creative work.

[0176] Through the description of the above embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus a necessary general hardware platform, and of course, it can also be implemented by hardware. Based on this understanding, the essence of the above technical solution, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0177] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements for some of the technical features; and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A path planning method, characterized in that, it includes: Determine the starting point and the target point of the path to be planned for the target object; Based on the starting point and the target point, determine the path to be planned according to the hybrid A* algorithm; In the hybrid A* algorithm, the cost function of the expanded node in the open list corresponding to the current node includes a turning cost; the turning cost is obtained based on the difference between the turning direction flag bit of the current node and the turning direction flag bit of the expanded node; The step of determining the path to be planned according to the hybrid A* algorithm includes: Perform the following processing steps on the current node: Generate the shortest path between the current node and the target point based on the kinematic constraints of the target object; Detect whether the target object collides with each target obstacle under the shortest path; If a collision occurs, generate the expanded nodes with different turning directions corresponding to the current node. If it is determined that the expanded node is not an obstacle, add the expanded node to the open list, select the expanded node with the minimum cost function in the open list as the new current node, and execute the processing steps; If no collision occurs, determine the path to be planned based on the shortest path.

2. The path planning method according to claim 1, characterized in that, The turning cost is the product of the absolute value of the difference and a preset coefficient.

3. The path planning method according to claim 1, characterized in that, The turning direction flag bit is a first flag bit for representing a left turn, a second flag bit for representing a straight line, or a third flag bit for representing a right turn.

4. The path planning method according to claim 1, characterized in that, The cost function is the sum of the path cost between the current node and the starting point, the path cost between the current node and the expanded node, the path cost between the expanded node and the target point, and the turning cost.

5. The path planning method according to claim 1, characterized in that, The step of detecting whether the target object collides with each target obstacle under the shortest path includes: For each path point in the shortest path, perform the following collision detection steps: Generate a first geometric figure enclosing the target object based on the size information of the target object, and generate a second geometric figure enclosing the target obstacle based on the size information of the target obstacle; In the world coordinate system, determine the coordinates of the target object based on the coordinates of the path point, solve the expression equation of the first geometric figure based on the coordinates of the target object and the size information of the target object, and solve the expression equation of the second geometric figure based on the coordinates of the target obstacle and the size information of the target obstacle; If it is determined that there is an intersection point belonging to the area enclosed by the second geometric figure between the expression equation of the first geometric figure and the expression equation of the second geometric figure, determine that the target object collides with the target obstacle, otherwise, determine that the target object does not collide with the target obstacle.

6. The path planning method according to claim 5, wherein, both the first geometric figure and the second geometric figure are polygons, and the expression equations of the first geometric figure and the second geometric figure are the expression equations of the straight lines where the sides of the polygon are located.

7. The path planning method according to claim 6, wherein, both the first geometric figure and the second geometric figure are rectangles, and the expression equations of the first geometric figure and the second geometric figure are the expression equations of the four straight lines where the sides of the rectangle are located.

8. The path planning method according to claim 1, wherein, before adding the expansion node to the open list if it is determined that the expansion node is not an obstacle, further includes: determining a safety distance in the grid coordinate system based on the grid accuracy of the grid coordinate system and the size information of the target object, expanding with the coordinates of the target obstacle as the center and the safety distance as the radius in the grid coordinate system, and determining the grid within the expanded area as the area of the virtual obstacle; if the expansion node is outside the area of the virtual obstacle, determining that the expansion node is not an obstacle.

9. A path planning device, wherein, comprises: a first determination module for determining the starting point and the target point of the path to be planned for the target object; a second determination module for determining the path to be planned based on the starting point and the target point according to the hybrid A* algorithm; in the hybrid A* algorithm, the cost function of the expansion node in the open list corresponding to the current node includes a turning cost; the turning cost is obtained based on the difference between the turning direction flag bit of the current node and the turning direction flag bit of the expansion node; the second determination module is specifically used for: performing the following processing steps on the current node: generating the shortest path between the current node and the target point based on the kinematic constraints of the target object; detecting whether the target object collides with each target obstacle under the shortest path; if a collision occurs, generating expansion nodes with different turning directions corresponding to the current node, adding the expansion node to the open list if it is determined that the expansion node is not an obstacle, selecting the expansion node with the smallest cost function in the open list as the new current node, and performing the processing steps; if no collision occurs, determining the path to be planned based on the shortest path.

10. The path planning device according to claim 9, wherein, the second determination module is specifically used for: performing the following collision detection steps for each path point in the shortest path: generating a first geometric figure that encloses the target object based on the size information of the target object, and generating a second geometric figure that encloses the target obstacle based on the size information of the target obstacle; In the world coordinate system, determine the coordinates of the target object based on the coordinates of the path points, solve the expression equation of the first geometric figure based on the coordinates of the target object and the size information of the target object, and solve the expression equation of the second geometric figure based on the coordinates of the target obstacle and the size information of the target obstacle; If it is determined that there is an intersection point belonging to the area enclosed by the second geometric figure in the expression equations of the first geometric figure and the second geometric figure, it is determined that the target object collides with the target obstacle; otherwise, it is determined that the target object does not collide with the target obstacle.

11. The path planning device according to claim 9, wherein, the second determination module is further configured to: Based on the grid accuracy of the grid coordinate system and the size information of the target object, determine the safety distance in the grid coordinate system. In the grid coordinate system, expand with the coordinates of the target obstacle as the center and the safety distance as the radius, and determine the grid within the expanded area as the area of the virtual obstacle; If the expanded node is outside the area of the virtual obstacle, it is determined that the expanded node is not an obstacle.

12. An electronic device, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein, when the processor executes the program, it implements the path planning method according to any one of claims 1 to 8.

13. A non-transitory computer-readable storage medium, on which a computer program is stored, wherein, when the computer program is executed by the processor, it implements the path planning method according to any one of claims 1 to 8.

14. A computer program product, comprising a computer program, wherein, when the computer program is executed by the processor, it implements the path planning method according to any one of claims 1 to 8.

Images