Path planning method and device

Abstract

The invention provides a path planning method and device, and the method comprises the steps: determining a map based on the environment of a target planning region, the map comprising a starting node, a target node and an obstacle; an improved A star algorithm is adopted to search from a starting node and a target node in two directions in sequence to obtain all intermediate nodes between the starting node and the target node, so that a global planning path is obtained, and all the intermediate nodes pass through all non-obstacle nodes in a planar neighborhood or a spatial neighborhood based on the previous nodes of the intermediate nodes; the path cost of each non-obstacle node is estimated by adopting a path cost function based on a safety penalty factor and a turning penalty factor; determining whether each included angle formed by each node in the global planning path and a previous node and a next node is within a set angle range or not to globally optimize the global planning path to obtain a globally optimized global planning path; and carrying out local path optimization on the globally optimized global planning path by adopting a dynamic window method.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a path planning method and device. Background Art

[0002] The goal of path planning is not only to find a feasible path, but more importantly, to optimize that path to meet specific performance metrics. The core task of path planning is to find the optimal path for a mobile entity from its starting point to its destination, taking into account various factors such as time, distance, cost, and safety.

[0003] Specifically, path planning involves planning a collision-free path to a destination that satisfies certain conditions in a static or dynamic environment. This includes both global and local path planning. Global path planning is a static, offline form of path planning. Given a mobile robot and a known environment model, the optimal path between the starting and final locations must be found without colliding with obstacles. Local path planning relies on a map with unknown obstacles. The main algorithms used for global path planning include Dijkstra's algorithm, the Rapidly Exploring Random Trees (RRT) algorithm, and the A* algorithm. The A* algorithm has been widely studied and applied due to its simple principle, ease of implementation, high search efficiency, and ability to find the optimal solution. However, the traditional A* algorithm suffers from the following challenges: low computational efficiency and time-consuming computation in large-scale environments; the generated path has large turns, is often not smooth, and is limited to two-dimensional path planning. Summary of the Invention

[0004] In view of this, embodiments of the present invention provide a path planning method and apparatus to eliminate or improve one or more defects in the prior art.

[0005] One aspect of the present invention provides a path planning method, the method comprising the following steps:

[0006] Determine a map based on the environment of the target planning area, wherein the map includes a starting node, a target node, and obstacles;

[0007] An improved A-star algorithm is used to search bidirectionally from a start node and a target node to obtain intermediate nodes between the start node and the target node, so as to obtain a global planning path. The intermediate nodes are determined by estimating the path cost of each non-obstacle node based on the non-obstacle nodes in the planar or spatial neighborhood of the previous node and using a path cost function based on a safety penalty factor and a turning penalty factor.

[0008] Determining whether each angle formed by each node in the global planning path and its respective previous node and next node is within a set angle range to globally optimize the global planning path and obtain a globally optimized global planning path;

[0009] The dynamic window method is used to perform local path optimization on the global planning path after global optimization, and the global planning path after global and local optimization is obtained.

[0010] In some embodiments of the present invention, the various non-obstruction nodes within the planar neighborhood of the previous node include various non-obstruction nodes at various angles adjacent to the previous node within the horizontal plane where the previous node is located, and the various non-obstruction nodes within the spatial neighborhood of the previous node include various non-obstruction nodes at various angles adjacent to the previous node within the vertical plane where the previous node is located and the horizontal plane where the previous node is located, which are determined based on the line between the previous node and the target node.

[0011] In some embodiments of the present invention, the path cost of the non-obstacle node includes the actual distance cost from the starting node or target node to the non-obstacle node and the heuristic estimated distance cost from the non-obstacle node to the target node or starting node, wherein the heuristic estimated distance cost includes the estimated distance between the non-obstacle node and the target node or starting node with the introduction of a weighting factor, a safety penalty factor, and a turning penalty factor.

[0012] In some embodiments of the present invention, the calculation formula of the path cost of the non-obstruction node is as follows:

[0013] f(n)=g(n)+h(n)

[0014] h(n)=λ·d+sp+tp

[0015] Where f(n) represents the path cost of the non-obstacle node, g(n) represents the actual distance cost from the start node or target node to the non-obstacle node, h(n) represents the heuristic estimated distance cost from the non-obstacle node to the target node or start node, λ represents the weighting factor, d represents the estimated distance from the non-obstacle node to the target node or start node, sp represents the safety penalty factor, and tp represents the turning penalty factor.

[0016] In some embodiments of the present invention, the safety penalty factor is obtained based on the minimum distance from the non-obstacle node to the nearby obstacle. If the minimum distance is less than a preset distance threshold, the value of the safety penalty factor is obtained by multiplying the minimum distance and the preset safety factor. Otherwise, the value of the safety penalty factor is 0; the turning penalty factor is obtained based on the angle formed by the non-obstacle node and the previous node, the previous node of the previous node, or the pitch angle of the robot during the search and movement process. If the angle exceeds the first angle threshold or the pitch angle exceeds the second angle threshold, the value of the turning penalty factor increases by 0.1 for every 1° increase in the excess part. Otherwise, the value of the turning penalty factor is 0.

[0017] In some embodiments of the present invention, determining whether each angle formed by each node in the global planning path and its respective previous node and next node is within a set angle range to globally optimize the global planning path to obtain a globally optimized global planning path includes:

[0018] For each node in the global planning path, if the angle formed by the node and the corresponding previous node and next node is within the set angle range, then the node is retained;

[0019] If the angles formed by a node and the corresponding previous node and next node are not within the set angle range, the node is determined to be a redundant node and is eliminated to obtain a globally optimized global planning path.

[0020] In some embodiments of the present invention, the method of performing local path optimization on the globally optimized global planning path using a dynamic window method to obtain the globally and locally optimized global planning path includes:

[0021] determining a dynamic window, wherein the dynamic window includes a plurality of groups of candidate speeds;

[0022] Based on multiple sets of candidate velocities, multiple local paths from each node in the globally optimized global planning path to each next node are simulated multiple times using a kinematic model within a preset prediction time to obtain multiple candidate local trajectories corresponding to each local path, wherein the velocities include linear velocities and angular velocities;

[0023] An evaluation function is used to determine the optimal local trajectory corresponding to each local path based on multiple candidate local trajectories corresponding to each local path, so as to obtain a global planning path after global and local optimization.

[0024] In some embodiments of the present invention, before the step of performing local path optimization on the globally optimized global planning path using the dynamic window method, the method further includes:

[0025] The globally optimized global planning path is smoothed to obtain a globally optimized global planning path that has been smoothed.

[0026] Another aspect of the present invention provides a path planning device, which includes: a computer device, the computer device includes a processor and a memory, the memory stores computer instructions, the processor is used to execute the computer instructions stored in the memory, and when the computer instructions are executed by the processor, the device implements the steps of the aforementioned method.

[0027] Another aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon, which is used to implement the steps of the aforementioned method when executed by a processor.

[0028] Another aspect of the present invention provides a computer program product comprising computer instructions, which implement the steps of the above method when executed by a processor.

[0029] The path planning method and device of the present invention can detect safe turning angles and perform global and local path optimization during the path planning process. They are applicable to path planning in both two-dimensional and three-dimensional space, and can improve the efficiency and effectiveness of two-dimensional and three-dimensional path planning. Furthermore, they effectively address issues such as low planning efficiency and time consumption in complex dynamic environments with large-scale obstacles, large turning angles in the generated paths, insufficient smoothness, poor real-time obstacle avoidance, and limitations to two-dimensional path planning, as seen in the traditional A-star algorithm.

[0030] Additional advantages, objects, and features of the present invention will be set forth in part in the following description and will become apparent to those skilled in the art upon examination of the following or may be learned from practice of the present invention. The objects and other advantages of the present invention may be realized and obtained by the structures particularly pointed out in the description and drawings.

[0031] Those skilled in the art will understand that the purposes and advantages that can be achieved by the present invention are not limited to the above specific descriptions, and the above and other purposes that can be achieved by the present invention will be more clearly understood based on the following detailed description. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] The drawings described herein are used to provide a further understanding of the present invention, constitute a part of this application, and do not constitute a limitation of the present invention.

[0033] Figure 1 Schematic diagram of a flow chart of a path planning method according to an embodiment of the present invention;

[0034] Figure 2 A schematic diagram of a node search direction in an embodiment of the present invention;

[0035] Figure 3 Schematic diagram of the path planning effect of a two-dimensional map in a scene with fixedly distributed obstacles in one embodiment of the present invention;

[0036] Figure 4 Schematic diagram of the path planning effect of a two-dimensional map in a randomly distributed obstacle scene according to one embodiment of the present invention;

[0037] Figure 5 Schematic diagram of the path planning effect of a three-dimensional map in a randomly distributed obstacle scene according to one embodiment of the present invention. DETAILED DESCRIPTION

[0038] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the embodiments and the accompanying drawings. Here, the exemplary embodiments of the present invention and their descriptions are used to explain the present invention, but are not intended to limit the present invention.

[0039] It should also be noted that, in order to avoid obscuring the present invention due to unnecessary details, the accompanying drawings only show structures and / or processing steps closely related to the solutions according to the present invention, while other details that are not closely related to the present invention are omitted.

[0040] It should be emphasized that the term "include / comprises" when used herein refers to the existence of features, elements, steps or components, but does not exclude the existence or addition of one or more other features, elements, steps or components.

[0041] It should also be noted that, unless otherwise specified, the term "connection" herein may refer not only to a direct connection but also to an indirect connection involving an intermediate.

[0042] Hereinafter, embodiments of the present invention will be described with reference to the accompanying drawings. In the accompanying drawings, the same reference numerals represent the same or similar components, or the same or similar steps.

[0043] To overcome the shortcomings of the traditional A* algorithm, the embodiments of the present invention propose a path planning method and device. During the path planning process, safe corner distance detection and global and local path optimization are performed. This method is applicable to path planning in two-dimensional planes and three-dimensional spaces and can improve the planning efficiency and effectiveness of two-dimensional and three-dimensional paths.

[0044] Figure 1 FIG. 1 is a flow chart of a path planning method according to an embodiment of the present invention. Figure 1 As shown, the method includes the following steps:

[0045] Step S110 : determining a map based on the environment of the target planning area, wherein the map includes a starting node, a target node, and obstacles.

[0046] Specifically, the grid method can be used to construct a two-dimensional map and / or a three-dimensional map based on the two-dimensional ground environment and / or three-dimensional space environment of the target planning area. Each grid cell or grid point in the map represents a traversable area or node in the environment. The obstacles in the two-dimensional map and the three-dimensional map can be concentrated in a fixed position, such as Figure 3 As shown, they can also be randomly distributed in different locations on the map, such as Figure 4 or Figure 5 And, set the starting point and end point of the final global planning path in the map, that is, the position of the starting node and the target node. For example, Figure 3 The fixed distribution shown and Figure 4 In the two-dimensional map of the randomly distributed obstacle scene shown in FIG, the position coordinates of the starting node (or starting point) are (0,0), and the position coordinates of the target node (or end point) are (55,55); Figure 5 In the 3D map of the randomly distributed obstacle scene shown, the starting point / starting node is at (0,0,0) and the ending point / target node is at (9,9,9). The starting and target nodes do not contain any obstacles.

[0047] In step S120, an improved A-star algorithm is used to search bidirectionally from the start node and the target node to obtain each intermediate node between the start node and the target node in sequence to obtain a global planning path, wherein each intermediate node is determined by estimating the path cost of each non-obstacle node based on each non-obstacle node in the plane neighborhood or spatial neighborhood of the previous node and using a path cost function based on a safety penalty factor and a turning penalty factor.

[0048] Furthermore, in step S120, the various non-obstacle nodes in the planar neighborhood of the previous node include various non-obstacle nodes at various angles adjacent to the previous node in the horizontal plane where the previous node is located, and the various non-obstacle nodes in the spatial neighborhood of the previous node include various non-obstacle nodes at various angles adjacent to the previous node in the vertical plane where the previous node is located and the horizontal plane where the previous node is located, which are determined based on the connection line between the previous node and the target node and are located.

[0049] In the process of planning the path and searching and expanding the intermediate nodes between the starting point and the end point, for the grid map, the node search direction of the traditional A* algorithm includes the following: Figure 2The angles formed by the parent node pointing to child nodes 1-8 are shown. In other words, each node is determined by searching for the child nodes at each of the angles above of the parent node. This method expands upon the traditional A* algorithm by expanding the search and expansion directions of nodes and performing a bidirectional parallel search from both the starting and ending points. This increases the search breadth and speed, improving the accuracy and efficiency of path planning.

[0050] Specifically, for path planning based on a two-dimensional map, each intermediate node is determined by searching for each non-obstruction node in the plane neighborhood of the previous node of each node. Among them, each non-obstruction node in the plane neighborhood of the previous node includes each non-obstruction node at each angle adjacent to the previous node in the horizontal plane where the previous node is located. For example, by searching for Figure 2 The parent node (the previous node of the intermediate node) is located in the horizontal plane where the child nodes 1-16 of each search direction formed by the parent node pointing to the child nodes 1-16 are searched and traversed, and the intermediate node is determined by obtaining the various non-obstacle nodes therein through obstacle identification. In this example, the search direction is sixteen search angles in the horizontal direction neighborhood. For path planning based on a three-dimensional map, each intermediate node is determined by searching for the various non-obstacle nodes in the spatial neighborhood of the previous node of each node. Among them, the various non-obstacle nodes of the previous node in the spatial neighborhood include the vertical plane of the previous node facing the target node determined based on the connection between the previous node and the target node and the various non-obstacle nodes at various angles adjacent to the previous node in the horizontal plane where the previous node is located. By selecting the various angles of the vertical plane neighborhood of the previous node facing the end point as the node search direction, the global path finally planned can reach the end point more quickly. For example, by performing the following operations on the path planning based on the three-dimensional map: Figure 2 The parent node is located in the horizontal plane and the parent node is located in the vertical plane. The parent node points to the child nodes 1-16 in each search direction formed by the child nodes 1-16, and the intermediate nodes are determined by identifying the non-obstacle nodes. In this example, the search direction includes sixteen search angles in the horizontal direction and sixteen search angles in the vertical direction. Figure 2 In the example shown, compared with the traditional A* algorithm, the present method adds eight search angles in the horizontal direction, 18°, 72°, 108°, 162°, 198°, 252°, 288° and 342°, or adds a total of sixteen search angles in the horizontal and vertical directions, 18°, 72°, 108°, 162°, 198°, 252°, 288° and 342°.

[0051] Next, for each intermediate node, the method uses a path cost function based on a safety penalty factor and a turning penalty factor to estimate the path cost of each non-obstruction node searched above to determine the corresponding intermediate node. Furthermore, the non-obstruction node with the smallest path cost is selected as the corresponding intermediate node. Furthermore, in step S120, the path cost of the non-obstruction node includes the actual distance cost from the start node or target node to the non-obstruction node and the heuristic estimated distance cost from the non-obstruction node to the target node or start node. The heuristic estimated distance cost includes the estimated distance between the non-obstruction node and the target node or start node, including a weighting factor, the safety penalty factor, and the turning penalty factor.

[0052] In a specific implementation of the present invention, the calculation formula of the path cost of the non-obstacle node, that is, the expression of the path cost function based on the safety penalty factor and the turning penalty factor, can be as follows:

[0053] f(n)=g(n)+h(n)

[0054] h(n)=λ·d+sp+tp

[0055] Among them, f(n) represents the path cost of the non-obstacle node, that is, the estimated value of the distance cost from the start node or target node to the target node or start node after passing through the non-obstacle node; g(n) represents the actual distance cost from the start node or target node to the non-obstacle node, h(n) represents the heuristic estimated distance cost from the non-obstacle node to the target node or start node, λ represents the weighting factor, d represents the estimated distance from the non-obstacle node to the target node or start node, sp represents the safety penalty factor, and tp represents the turning penalty factor.

[0056] Specifically, the estimated distance d between the non-obstacle node and the target node or the starting node can be calculated using the Chebyshev distance, which is defined as follows:

[0057] d=max(|x1-x2|,|y1-y2|)

[0058] d=max(|x1-x2|,|y1-y2|,|z1-z2|)

[0059] Among them, (x1, y1) or (x1, y1, z1) represents the position coordinates of the non-obstacle node, and (x2, y2) or (x2, y2, z2) represents the position coordinates of the target node or the starting node.

[0060] The safety penalty factor is based on the minimum distance between a non-obstruction node and a nearby obstacle. More specifically, if the minimum distance between a non-obstruction node and a nearby obstacle is less than a preset distance threshold, the safety penalty factor is calculated by multiplying the minimum distance by a preset safety factor, which can be set based on the safety requirements of different tasks. If the minimum distance between a non-obstruction node and a nearby obstacle is greater than or equal to the preset distance threshold, the safety penalty factor is zero.

[0061] The turning penalty factor is obtained based on the angle formed by the non-obstacle node with the previous node, the previous node of the previous node, or the pitch angle of the robot during the search process. More specifically, if the angle exceeds the first angle threshold or the pitch angle exceeds the second angle threshold, the value of the turning penalty factor increases by 0.1 for every 1° increase in the excess portion; otherwise, the value of the turning penalty factor is 0. When the angle formed by the non-obstacle node with the previous node, the previous node of the previous node is greater than the first angle threshold, the path passing through the non-obstacle node is considered to be a sharp turn. Exemplarily, the first angle threshold can be 45° and the second angle threshold can be 15°.

[0062] The path cost function based on the safety penalty factor and the turning penalty factor proposed in this method is a composite heuristic function for multi-objective optimization. The multi-objective optimization includes the optimization of the safety distance cost, the path turning penalty, and the target tendency factor (weighted factor). By introducing a weighted factor into the estimated distance between the non-obstacle node and the target node or the starting node, the weight ratio of the unknown path cost (estimated distance) in the path cost value of the non-obstacle node in the total path cost can be increased. At the same time, by introducing the safety penalty factor and the turning penalty factor into consideration in the determination process of each intermediate node, the distance between the non-obstacle node and its nearest obstacle is considered when estimating the path cost of the non-obstacle node, thereby achieving adaptive safety distance maintenance between the path and the obstacle. This allows the robot to achieve safe cornering and travel according to the planned global path, avoid sharp turns, improve the safety of the path travel, and reduce the risk of loss of control.

[0063] Based on this, the search process in step S120 specifically includes: starting with a bidirectional parallel search at the start node and the target node, using the previous node of each intermediate node (including the start node and the target node) as the parent node, and sequentially performing node search and expansion, as well as obstacle detection, according to each search direction corresponding to the previously set two-dimensional path planning or three-dimensional path planning. Each search and expansion selects the non-obstruction node with the lowest path cost as the determined intermediate node, gradually obtaining each intermediate node until the bidirectional search encounters a node. This can improve path calculation efficiency and path security. During this process, two queues are defined: a queue to be searched (OpenList) and a queue to be searched (CloseList). The OpenList is used to store the nodes to be searched. For two-dimensional path planning, the nodes to be searched include the non-obstruction nodes within the planar neighborhood of the previous node of each intermediate node; for three-dimensional path planning, the nodes to be searched include the non-obstruction nodes within the spatial neighborhood of the previous node of each intermediate node. The CloseList is used to store the non-obstruction nodes and obstacle nodes that have been searched and determined as intermediate nodes. Initialize the path cost values of the start and target nodes to 0 and add them to the OpenList. Search for non-obstruction nodes in the neighborhoods corresponding to the start and target nodes, add them to the OpenList, calculate the path cost for each non-obstruction node, and then select the two non-obstruction nodes with the smallest path cost as the next intermediate nodes corresponding to the start and target nodes. Then, continue searching for non-obstruction nodes in the neighborhoods corresponding to these two next intermediate nodes, add them to the OpenList, calculate the path cost for each non-obstruction node, and then select the two non-obstruction nodes with the smallest path cost to obtain the next intermediate node of the next intermediate node corresponding to the start and target nodes. This process repeats itself until all intermediate nodes between the start and target nodes are found. Among them, if a non-obstruction node in a neighborhood is already in the CloseList, it can be ignored; if a non-obstruction node in a neighborhood is not in the OpenList, it is added to the OpenList, and the values of g(n) and h(n) are calculated to obtain the path cost value; if a non-obstruction node in a neighborhood is already in the OpenList, check whether the path distance from its parent node to the neighboring non-obstruction node is shorter. If it is shorter, update the g(n) value until the two-way encounter.

[0064] Step S130 , determining whether each angle formed by each node in the global planning path and its respective previous node and next node is within a set angle range to globally optimize the global planning path and obtain a globally optimized global planning path.

[0065] Specifically, step S130, determining whether each angle formed by each node in the global planning path and its respective previous node and next node is within a set angle range to globally optimize the global planning path, and obtaining a globally optimized global planning path, includes the following steps:

[0066] For each node in the global planning path, if the angle formed by the node and the corresponding previous node and next node is within the set angle range, then the node is retained;

[0067] If the angles formed by a node and the corresponding previous node and next node are not within the set angle range, the node is determined to be a redundant node and is eliminated to obtain a globally optimized global planning path.

[0068] Specifically, all nodes can be traversed from their respective starting points in the two-dimensional and / or three-dimensional global planning path and calculations and judgments can be performed. For each node, the angle between the node and the previous node (previous node) and the next node (next node) is calculated, and the angle is determined to be within the set angle range to identify redundant nodes. By identifying and eliminating all redundant nodes in the global planning path, the problem of too many turning points in the global planning path can be solved, and global optimization of the global planning path can be achieved, which can be as follows: Figure 3 or Figure 4 The two-dimensional global planning path after global optimization is shown in green. For example, the set angle range can be greater than 90° and less than 180°.

[0069] Step S140 , using a dynamic window method to perform local path optimization on the globally optimized global planning path, to obtain a globally and locally optimized global planning path.

[0070] Specifically, step S140, wherein the dynamic window method is used to perform local path optimization on the global planning path after global optimization to obtain the global planning path after global and local optimization, includes the following steps:

[0071] determining a dynamic window, wherein the dynamic window includes a plurality of groups of candidate speeds;

[0072] Based on multiple sets of candidate velocities, multiple local paths from each node in the globally optimized global planning path to each next node are simulated multiple times using a kinematic model within a preset prediction time to obtain multiple candidate local motion trajectories corresponding to each local path, wherein the velocities include linear velocities and angular velocities;

[0073] An evaluation function is used to determine the optimal local motion trajectory corresponding to each local path based on multiple candidate local motion trajectories corresponding to each local path, so as to obtain a global planning path after global and local optimization.

[0074] Specifically, the dynamic window method (DWA) generates multiple sets of candidate velocities according to the speed and acceleration limits of the robot used to simulate the travel path to form a dynamic window, wherein each set of candidate velocities can be a combination of candidate linear velocities and angular velocities. Based on the motion trajectory of each local path in the two-dimensional and / or three-dimensional global planning path after global optimization within the prediction time of each set of candidate velocities, multiple candidate local motion trajectories corresponding to each local path are obtained, and the multiple candidate local motion trajectories corresponding to each local path are evaluated using an evaluation function, and the optimal local motion trajectory corresponding to each local path is selected, thereby obtaining an executable trajectory that is more in line with the robot's motion characteristics, that is, the global planning path after global and local optimization. Among them, the evaluation function can be defined as shown in the following formula:

[0075] Cost(v,ω)=αHead(v,ω)+βObs(v,ω)+φVel(v,ω)

[0076] Here, Cost(v,ω) represents the cost evaluation of the candidate local motion trajectory; Head(v,ω) represents the angle between the line connecting the robot's current position and the target node and the heading of the current position. The smaller the angle, the smaller the Head value. Obs(v,ω) represents the minimum distance between the robot and nearby obstacles. The larger the minimum distance, the smaller the Obs value. Vel(v,ω) represents the simulated set of candidate velocity costs. The larger the candidate velocity, the smaller the Vel value. v represents the candidate linear velocity, ω represents the candidate angular velocity, and α, β, and φ represent weighting coefficients. The smaller the value of the evaluation function, the better the candidate local motion trajectory. By evaluating the heading, obstacle distance, and speed cost of multiple candidate local motion trajectories for each local path, the optimal local motion trajectory of each selected local path is selected to form an optimized global planning path that not only safely travels toward the target node, avoiding collisions with obstacles, but also reaches the target node more quickly.

[0077] In one embodiment of the present invention, before the step of performing local path optimization on the globally optimized global planning path using the dynamic window method, the path planning method further includes the following steps:

[0078] The globally optimized global planning path is smoothed to obtain a globally optimized global planning path that has been smoothed.

[0079] Specifically, the smoothing of the two-dimensional and / or three-dimensional global planning path obtained after global optimization in step S130 can be performed using a B-spline curve, thereby improving the smoothness of the global planning path and the stability of the robot motion. Specifically, the B-spline curve is a spline curve whose direction and range are determined by a series of control points. The nodes in the global planning path after global optimization are used as the control points of the spline curve to smooth the global planning path. Assume that P0, P1, P2, ..., P n There are a total of n+1 control points (path nodes), which are used to define the direction and limit range of the spline curve. The definition of the k-order B-spline curve with n+1 control points is as follows:

[0080]

[0081] Among them, B i,k (u) represents the i-th k-order B-spline basis function, and the control point P i Correspondingly, u is an independent variable, and its order is not less than 1. In this embodiment, the global and locally optimized global planning paths obtained after smoothing and local path optimization in step S140 can be as follows: Figure 3 or Figure 4 The two-dimensional global planning path is shown by the red line in .

[0082] After experiments and tests, the method was tested multiple times in a scenario where obstacles were randomly generated dynamically and the obstacle density was limited. The global path planning effect from the starting node to the target node obtained by the test can be shown as follows: Figure 4 or Figure 5 This shows that the method can adapt well to dynamic environments. In the aerial simulation experiment in the three-dimensional obstacle map, the global path planning effect from the starting node to the target node can be shown as follows: Figure 5 This shows that this method is suitable for the planning needs of three-dimensional aerial paths of flying cars, etc. Figures 3 to 5 During the experimental testing in the three scenarios shown, this method effectively avoided obstacles during travel, meeting the needs of flying cars and other applications for finding optimal paths both on the ground and in the air. Furthermore, the path planning method and device proposed in this embodiment of the present invention can be widely applied to applications requiring path planning in complex environments, such as autonomous driving and robot navigation.

[0083] Corresponding to the above method, the present invention also provides a path planning device, which includes a computer device, the computer device includes a processor and a memory, the memory stores computer instructions, and the processor is used to execute the computer instructions stored in the memory. When the computer instructions are executed by the processor, the device implements the steps of the above method.

[0084] An embodiment of the present invention further provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the aforementioned method. The computer-readable storage medium may be a tangible storage medium, such as a random access memory (RAM), a memory, a read-only memory (ROM), an electrically programmable ROM, an electrically erasable programmable ROM, a register, a floppy disk, a hard disk, a removable storage disk, a CD-ROM, or any other form of storage medium known in the art.

[0085] An embodiment of the present invention further provides a computer program product, comprising computer instructions, which implement the steps of the aforementioned method when executed by a processor.

[0086] It should be understood by those skilled in the art that the various exemplary components, systems and methods described in conjunction with the embodiments disclosed herein can be implemented in hardware, software or a combination of the two. Whether it is specifically performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention. When implemented in hardware, it can be, for example, an electronic circuit, an application specific integrated circuit (ASIC), appropriate firmware, a plug-in, a function card, etc. When implemented in software, the elements of the present invention are programs or code segments that are used to perform the required tasks. The program or code segment can be stored in a machine-readable medium, or transmitted on a transmission medium or a communication link via a data signal carried in a carrier.

[0087] It should be understood that the present invention is not limited to the specific configurations and processes described above and illustrated in the figures. For the sake of brevity, a detailed description of known methods is omitted. In the above embodiments, several specific steps are described and illustrated as examples. However, the method of the present invention is not limited to the specific steps described and illustrated. Those skilled in the art may make various changes, modifications, and additions, or change the order of the steps after understanding the spirit of the present invention.

[0088] In the present invention, features described and / or illustrated for one embodiment may be used in the same or similar manner in one or more other embodiments, and / or combined with or replace features of other embodiments.

[0089] The foregoing description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Those skilled in the art will readily appreciate that various modifications and variations to the present invention are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention are intended to be within the scope of protection of the present invention.

Claims

1. A path planning method, characterized in that: The method comprises: Determine a map based on the environment of the target planning area, wherein the map includes a starting node, a target node, and obstacles; An improved A-star algorithm is used to search bidirectionally from a start node and a target node to obtain intermediate nodes between the start node and the target node, so as to obtain a global planning path. The intermediate nodes are determined by estimating the path cost of each non-obstacle node based on the non-obstacle nodes in the planar or spatial neighborhood of the previous node and using a path cost function based on a safety penalty factor and a turning penalty factor. Determining whether each angle formed by each node in the global planning path and its respective previous node and next node is within a set angle range to globally optimize the global planning path and obtain a globally optimized global planning path; The dynamic window method is used to perform local path optimization on the global planning path after global optimization, and the global planning path after global and local optimization is obtained.

2. The method according to claim 1, characterized in that The non-obstruction nodes of the previous node in the planar neighborhood include the non-obstruction nodes at various angles adjacent to the previous node in the horizontal plane where the previous node is located, and the non-obstruction nodes of the previous node in the spatial neighborhood include the non-obstruction nodes at various angles adjacent to the previous node in the vertical plane where the previous node is located and the horizontal plane where the previous node is located, which are determined based on the connection line between the previous node and the target node.

3. The method according to claim 1, characterized in that The path cost of the non-obstacle node includes the actual distance cost from the starting node or the target node to the non-obstacle node and the heuristic estimated distance cost from the non-obstacle node to the target node or the starting node, wherein the heuristic estimated distance cost includes the estimated distance between the non-obstacle node and the target node or the starting node with the introduction of a weighting factor, a safety penalty factor, and a turning penalty factor.

4. The method according to claim 3, characterized in that The calculation formula of the path cost of the non-obstacle node is as follows: f(n)=g(n)+h(n) h(n)=λ·d+sp+tp Where f(n) represents the path cost of the non-obstacle node, g(n) represents the actual distance cost from the start node or target node to the non-obstacle node, h(n) represents the heuristic estimated distance cost from the non-obstacle node to the target node or start node, λ represents the weighting factor, d represents the estimated distance from the non-obstacle node to the target node or start node, sp represents the safety penalty factor, and tp represents the turning penalty factor.

5. The method according to claim 1, 3 or 4, characterized in that The safety penalty factor is obtained based on the minimum distance from the non-obstacle node to the nearby obstacle. If the minimum distance is less than the preset distance threshold, the value of the safety penalty factor is obtained by multiplying the minimum distance and the preset safety factor. Otherwise, the value of the safety penalty factor is 0. The turning penalty factor is obtained based on the angle formed by the non-obstacle node and the previous node, the previous node of the previous node, or the pitch angle of the robot during the search and movement process. If the angle exceeds the first angle threshold or the pitch angle exceeds the second angle threshold, the value of the turning penalty factor increases by 0.1 for every 1° increase in the excess part. Otherwise, the value of the turning penalty factor is 0.

6. The method according to claim 1, characterized in that Determining whether each angle formed by each node in the global planning path and its respective previous node and next node is within a set angle range to globally optimize the global planning path, and obtaining a globally optimized global planning path, includes: For each node in the global planning path, if the angle formed by the node and the corresponding previous node and next node is within the set angle range, then the node is retained; If the angles formed by a node and the corresponding previous node and next node are not within the set angle range, the node is determined to be a redundant node and is eliminated to obtain a globally optimized global planning path; The method of using the dynamic window method to perform local path optimization on the global planning path after global optimization to obtain the global planning path after global and local optimization includes: determining a dynamic window, wherein the dynamic window includes a plurality of groups of candidate speeds; Based on multiple sets of candidate velocities, multiple local paths from each node in the globally optimized global planning path to each next node are simulated multiple times using a kinematic model within a preset prediction time to obtain multiple candidate local trajectories corresponding to each local path, wherein the velocities include linear velocities and angular velocities; An evaluation function is used to determine the optimal local trajectory corresponding to each local path based on multiple candidate local trajectories corresponding to each local path, so as to obtain a global planning path after global and local optimization.

7. The method according to any one of claims 1 to 4 and 6, characterized in that Before the step of performing local path optimization on the globally optimized global planning path using the dynamic window method, the method further includes: The globally optimized global planning path is smoothed to obtain a globally optimized global planning path that has been smoothed.

8. A path planning device comprising a processor, a memory, and computer instructions stored in the memory, characterized in that: The processor is configured to execute the computer instructions. When the computer instructions are executed, the device implements the steps of the method according to any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A computer program product comprising computer instructions, characterized in that When the computer instructions are executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images