Ground path planning method and device

Abstract

The invention provides a ground path planning method and device, and the method comprises the steps: determining a two-dimensional map based on the ground environment of a target planning region, the two-dimensional map comprising a starting node, a target node and an obstacle; sequentially searching from the starting node by adopting an improved A star algorithm to obtain each intermediate node between the starting node and the target node so as to obtain a planned path, each intermediate node is determined by estimating the cost of each non-obstacle node on the basis of each non-obstacle node adjacent to the previous node of the intermediate node and by adopting a heuristic function based on a dynamic weight; determining whether every three continuous nodes in the planned path in the direction from the starting node to the target node are collinear or not or whether a connecting line between two nodes except the node located in the middle in the three nodes does not penetrate through an obstacle or not to globally optimize the planned path, and obtaining a globally optimized planned path; and carrying out local path optimization on the globally optimized 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 ground path planning method and device. Background Art

[0002] When planning a flying car's ground path, avoiding obstacles and ensuring safe flight are paramount. Considering factors such as the flying car's inherent energy consumption, the path should be as short as possible, with smooth turns and a safe distance from obstacles. Path planning for a flying car requires a clear starting and ending point, as well as obstacle avoidance. The quality of path planning directly determines its accuracy and efficiency.

[0003] Commonly used global path planning algorithms include A* algorithm (A star algorithm), Rapidly-exploring Random Trees (RRT) and Particle Swarm Optimization (PSO). Among them, the A* algorithm has become one of the most commonly used global planning methods due to its advantages such as fast calculation speed and the ability to find the shortest path. However, the inaccuracy of the heuristic function of the A* algorithm and the complexity of the environment will lead to an increase in the search space. In a complex environment, the A* algorithm needs to expand nodes on a large scale to ensure that the optimal solution for the path can be found in order to ensure the shortest path. This will result in a long search time and the presence of many unnecessary nodes. During the search process, the traditional A* algorithm has many search directions on the grid map, such as Figure 1 As shown in FIG, the angle between any search direction and the x-axis is an integer multiple of 45°. This angle restriction increases the turning points, making the planned path longer, and the turning amplitude is large, and the path is not smooth.

[0004] Considering the large size and energy consumption of flying cars, the traditional A* algorithm is not conducive to completing the path planning task of flying cars. Summary of the Invention

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

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

[0007] Determine a two-dimensional map based on the ground environment of the target planning area, wherein the two-dimensional map includes a starting node, a target node, and obstacles;

[0008] An improved A-star algorithm is used to sequentially search for intermediate nodes between the starting node and the target node to obtain a planned path. The intermediate nodes are determined by estimating the cost of each non-obstruction node based on the adjacent non-obstruction nodes of the previous node and a heuristic function based on dynamic weights.

[0009] Determining whether every three consecutive nodes in the planned path along the direction from the start node to the target node are collinear or whether a line between two nodes other than the middle node among the three nodes does not pass through an obstacle to globally optimize the planned path, thereby obtaining a globally optimized planned path;

[0010] The dynamic window method is used to perform local path optimization on the globally optimized planning path to obtain the globally and locally optimized planning paths.

[0011] In some embodiments of the present invention, the non-obstacle nodes adjacent to the previous node include: the angle between the straight line between the previous node and the target node and the positive x-axis is in the range of 0° to 90°, 90° to 180°, 180° to 270° or 270° to 360°, and the corresponding non-obstacle nodes in the area facing the target node adjacent to the previous node.

[0012] In some embodiments of the present invention, the cost of the non-obstacle node includes the moving cost from the starting node to the non-obstacle node and the estimated moving cost from the non-obstacle node to the target node with dynamic weights, wherein the farther the non-obstacle node is from the target node, the greater the weight of the estimated moving cost, and the closer the non-obstacle node is to the target node, the smaller the weight of the estimated moving cost, until it drops to 0.

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

[0014] f(n)=g(n)+w(n)h(n)

[0015]

[0016] Where f(n) represents the cost of the non-obstruction node, g(n) represents the movement cost from the starting node to the non-obstruction node, h(n) represents the estimated movement cost from the non-obstruction node to the target node, w(n) represents the dynamic weight, D represents the cost from the non-obstruction node to the target node, and T represents the cost from the starting node to the target node.

[0017] In some embodiments of the present invention, the movement cost from the starting node to the non-obstacle node is obtained by calculating the Euclidean distance between the starting node and the non-obstacle node, and the estimated movement cost from the non-obstacle node to the target node is obtained by calculating the Manhattan distance between the non-obstacle node and the target node.

[0018] In some embodiments of the present invention, determining whether every three consecutive nodes in the planned path along the direction from the start node to the target node are collinear or whether a line between two of the three nodes, except for the middle node, does not pass through an obstacle to globally optimize the planned path, thereby obtaining the globally optimized planned path, includes:

[0019] If every three consecutive nodes in the planned path along the direction from the starting node to the target node are collinear or the line between two nodes other than the middle node among the three nodes does not pass through an obstacle, then each middle node is determined to be a redundant node and the redundant nodes are deleted to obtain a globally optimized planned path.

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

[0021] Extract key nodes based on the globally optimized planned path to obtain multiple key nodes;

[0022] Determine a dynamic window, the dynamic window including a velocity constraint range, and simulate each local path for a period of time multiple times using a kinematic model with each key node as a starting point based on each set of velocities within the velocity constraint range to obtain multiple candidate local trajectories corresponding to each local path, wherein the velocities include linear velocity and angular velocity;

[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 globally and locally optimized planned path.

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

[0025] Smoothing the globally optimized planned path to obtain the globally optimized planned path trajectory;

[0026] Correspondingly, the dynamic window method is used to perform local path optimization on the globally optimized planned path trajectory to obtain the globally and locally optimized planned path trajectory.

[0027] Another aspect of the present invention provides a ground 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.

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

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

[0030] The ground path planning method and device of the present invention can generate a more optimal and shorter planned path while minimizing turning points with large turning amplitudes. This can effectively reduce steering energy consumption, especially the steering energy consumption of larger equipment such as flying cars, and improve the movement flexibility of large equipment, avoid collisions with obstacles, and effectively reduce movement risks. It is suitable for various application scenarios such as intelligent driving and ground navigation in complex terrain such as urban roads.

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

[0032] 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

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

[0034] Figure 1 This is a schematic diagram of the search direction of the traditional A* algorithm on a grid map;

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

[0036] Figure 3 Schematic diagram of a specific process of a ground path planning method in one embodiment of the present invention. DETAILED DESCRIPTION

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

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

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

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

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

[0042] In order to plan the optimal path, that is, the shortest path, while minimizing turning points with large turning amplitudes and avoiding an uneven path, the embodiments of the present invention propose a ground path planning method and device. These methods can effectively reduce steering energy consumption, especially for larger equipment such as flying cars, and improve the movement flexibility of large equipment, avoiding collisions with obstacles, and effectively reducing movement risks. The method and device are suitable for various application scenarios such as intelligent driving and ground navigation in complex terrain such as urban roads, and have broad application potential.

[0043] Figure 2 and Figure 3 They are respectively a flow chart and a specific flow chart of a ground path planning method in one embodiment of the present invention. Figure 2 and Figure 3 As shown, the method includes the following steps:

[0044] Step S210 : determining a two-dimensional map based on the ground environment of the target planning area, wherein the two-dimensional map includes a starting node, a target node, and obstacles.

[0045] Specifically, this step utilizes LiDAR and the SLAM algorithm to construct a two-dimensional grid map of the target planning area, referencing the actual ground environment of the target planning area. The map then determines the locations of the starting node (starting point) and the target node (end point). All obstacles on the two-dimensional grid map are identified and marked. Each grid cell in the two-dimensional grid map represents a movable or walkable node in the environment.

[0046] In step S220, the improved A-star algorithm is used to search sequentially from the starting node to obtain the intermediate nodes between the starting node and the target node to obtain the planned path, wherein the intermediate nodes are determined by estimating the cost of each non-obstacle node based on the adjacent non-obstacle nodes of their respective previous nodes and using a heuristic function based on dynamic weights.

[0047] Furthermore, in step S220, each non-obstruction node adjacent to the previous node includes each non-obstruction node adjacent to the previous node and within an area facing the target node.

[0048] In the traditional A* algorithm, the node search and expansion process is to search in 8 directions around the current node, that is, the parent node. The 8 directions are as follows: Figure 1 Nodes 1-8 shown. As long as there is no obstruction, that is, these 8 nodes are not where obstacles are located, the flying car can move to the child nodes in these 8 directions. Compared with the traditional A* algorithm, the direction of node search and expansion in the improved A star algorithm in step S220 is different. Specifically, according to the angle between the straight line between the previous node and the target node and the positive x-axis, each non-obstacle node adjacent to the previous node in the area facing the target node is selected for search and expansion to obtain each intermediate node. When the angle is in the range of 0° to 90°, the nodes selected for expansion and search are as follows Figure 1 The subnodes 1, 2, 3, 4 and 8 shown are the subnodes in the area facing the target node with subnodes 4 and 8 as the diagonal line; when the angle is within the range of 90° to 180°, the nodes selected for expansion and search are as follows Figure 1 The subnodes 2, 3, 4, 6 and 6 shown are the subnodes in the area facing the target node with subnodes 2 and 6 as the diagonal line; when the angle is in the range of 180° to 270°, the nodes selected for expansion and search are as follows Figure 1 The subnodes 4, 5, 6, 7 and 8 shown are the subnodes in the area facing the target node with the subnodes 4 and 8 as the diagonal line; when the angle is in the range of 270° to 360°, the nodes selected for expansion and search are as follows Figure 1The subnodes 1, 2, 6, 7 and 8 shown are the subnodes in the area facing the target node with subnodes 2 and 6 as the diagonal. Within the above angle ranges, if a subnode among the corresponding subnodes selected for expansion and search is an obstacle, the subnode will not be considered and will be regarded as an unreachable node. By selecting nodes within the corresponding range for search and expansion based on the geometric constraints of the above angles, the search range can be narrowed, and unnecessary node expansion can be significantly reduced. While maintaining the optimal path, invalid turning attempts can be reduced, thereby improving search efficiency. This can meet application scenarios such as ground navigation with higher real-time requirements.

[0049] In the traditional A* algorithm, for the current node, a heuristic function is used to evaluate the costs of expanding and searching child nodes in eight directions. The child node with the lowest cost is selected as the node in the planned path. This node is then used as the parent node for the next round of search, and the search process repeats until the target node is found. The heuristic function is f(n) = g(n) + h(n), where g(n) represents the cost of moving from the starting node to the expanded and searched child nodes, and h(n) represents the estimated cost of moving from the expanded and searched child nodes to the target node. g(n) plays a dominant role. A smaller proportion of h(n) ensures the shortest path is found, improving path accuracy, but also increases the number of expanded and searched nodes, thus prolonging the search time. A larger proportion of h(n) increases the algorithm's heuristic power, favoring nodes close to the target node and increasing search speed, but it cannot guarantee the shortest path. Therefore, embodiments of the present invention significantly improve algorithm efficiency by designing an appropriate calculation strategy to dynamically control the proportions of g(n) and h(n) in the total cost function during the search process. Therefore, in the improved A-star algorithm in step S220, a heuristic function based on dynamic weights is proposed, and the heuristic function based on dynamic weights is used to estimate the cost of each non-obstacle node to obtain each intermediate node.

[0050] Based on this, the search process in step S220 specifically includes: starting from the start node, expanding and searching the corresponding five child nodes based on the range of the angle between the straight line between the start node and the target node and the positive x-axis, detecting whether the five child nodes are obstacle nodes, and adding each non-obstacle node in the search list (OPEN list). Then, using a heuristic function based on dynamic weights to estimate the cost of each non-obstacle node in the OPEN list, the non-obstacle node with the lowest or minimum cost is selected as the first intermediate node after the start node, and this first intermediate node is added to the search list (CLOSED list). Next, using the first intermediate node as the parent node, traversing each child node in the parent node's adjacent region facing the target node, that is, expanding and searching the corresponding five child nodes based on the range of the angle between the straight line between the first target node and the target node and the positive x-axis, and similarly detecting whether the five child nodes are obstacle nodes, and adding each non-obstacle node in the search list (if the non-obstacle node being added is already in the OPEN list, it does not need to be added again). Next, a dynamic weighted heuristic function is used to estimate the cost of each non-obstruction node in this expansion. The non-obstruction node with the lowest cost is selected as the second intermediate node and added to the closed list. A similar process is then repeated until the target node appears in the closed list. This means that all intermediate nodes between the start and target nodes have been found, indicating that the planned path has been obtained.

[0051] Specifically, the cost of the non-obstruction node includes the movement cost from the starting node to the non-obstruction node and the estimated movement cost from the non-obstruction node to the target node, which is dynamically weighted. The farther the non-obstruction node is from the target node, the greater the weight of the estimated movement cost, allowing for faster progress toward the target node during path planning. The closer the non-obstruction node is to the target node, the smaller the weight of the estimated movement cost, until it drops to 0, ensuring rapid arrival at the target node during path planning. By dynamically adjusting the weight coefficients in the heuristic function, which automatically adjust with distance from the target node, the improved A* algorithm can more flexibly balance the "actual movement cost" and "estimated movement cost" during path search. This allows the algorithm to prioritize straighter path segments in complex terrain, reducing energy consumption caused by frequent turns while avoiding local turning point traps. This reduces the frequency of path turnings from a search strategy perspective. Furthermore, this approach ensures path optimality while improving search speed.

[0052] In a specific implementation process, the calculation formula of the cost of the non-obstruction node, that is, the expression of the heuristic function based on the dynamic weight, can be as follows:

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

[0054]

[0055] Here, f(n) represents the cost of a non-obstruction node, g(n) represents the cost of moving from the starting node to a non-obstruction node, h(n) represents the estimated cost of moving from a non-obstruction node to a target node, w(n) represents the dynamic weight, D represents the cost of moving from a non-obstruction node to a target node, and T represents the cost of moving from the starting node to the target node. In this implementation, an exponential function is used to dynamically adjust the weights in the heuristic function. This allows the improved A* algorithm to quickly approach the target node when planning a path far from it and accurately converge when approaching it, resolving the issue with fixed weights that prevents the method from balancing efficiency and accuracy.

[0056] Specifically, the moving cost from the starting node to the non-obstruction node can be obtained by calculating the Euclidean distance between the starting node and the non-obstruction node, and the estimated moving cost from the non-obstruction node to the target node can be obtained by calculating the Manhattan distance between the non-obstruction node and the target node. The specific formulas can be expressed as:

[0057]

[0058] h(n)=|x g -x n |+|y g -y n |

[0059] Among them, (x s ,y s ) represents the position coordinates of the starting node, (x n ,y n ) represents the position coordinates of the non-obstacle node, (x g ,y g ) represents the location coordinates of the target node.

[0060] Step S230, determining whether every three consecutive nodes in the planned path along the direction from the starting node to the target node are collinear or whether the connection between two nodes among the three nodes except the node in the middle does not pass through an obstacle to globally optimize the planned path and obtain a globally optimized planned path.

[0061] Specifically, step S230, determining whether every three consecutive nodes in the planned path along the direction from the start node to the target node are collinear or whether the line between two nodes other than the middle node among the three nodes does not pass through an obstacle to globally optimize the planned path, and obtaining the globally optimized planned path, includes the following steps:

[0062] If every three consecutive nodes in the planned path along the direction from the starting node to the target node are collinear or the line between two nodes other than the middle node among the three nodes does not pass through an obstacle, then each middle node is determined to be a redundant node and the redundant nodes are deleted to obtain a globally optimized planned path.

[0063] Because the improved A-star algorithm in this method performs path search on a node-by-node basis, the final searched and formed planned path may contain many unnecessary turning points, resulting in the formed planned path not being the shortest path or the optimal path. To solve this problem, this method can combine the greedy search method to first determine the nodes in the searched list, that is, the planned path, thereby performing global optimization of the planned route. In a specific implementation, the specific process of determination may include the following steps:

[0064] Step 1.1: Assume that the planned path or the CLOSED list contains n nodes, denoted as P1, P2, ..., Pi, ..., Pn;

[0065] Step 1.2: Determine whether the three consecutive nodes P1, P2, and P3 are located on the same straight line. If so, node P2 is considered redundant and is deleted from the searched list. The path from P1 to P3 is returned. If not, determine whether the line connecting nodes P1 and P3 passes through an obstacle. If the line connecting nodes P1 and P3 passes through an obstacle, it indicates that the path is unsafe to pass; if it does not pass through an obstacle, it indicates that the path is safe to pass.

[0066] Step 1.3: If not, node P2 is a redundant node, and the redundant node P2 is deleted from the searched list, and the path from P1 to P3 is returned; if so, the path from P1 to P2 and the path from P2 to P3 are returned;

[0067] Step 1.4: Repeat steps 1.1 to 1.3 to judge the three consecutive nodes P2, P3, P4, ..., Pn-2, Pn-1, Pn in turn, that is, until all three consecutive nodes in the CLOSED list are searched.

[0068] The above steps, by applying the greedy search method to the present invention, perform post-path optimization on the planned path formed by all nodes in the CLOSED list. By determining the reachability of straight lines between nodes, redundant turning points are directly deleted. Experimental data confirms that this improves the path straightness by approximately 30%, significantly reducing the energy loss caused by mechanical steering, and effectively solving the redundancy problem of planned paths generated by the traditional A* algorithm. By combining the global planning capabilities of the improved A* algorithm with the optimization capabilities of the greedy search method, this method overcomes the limitations of the traditional A* algorithm in balancing path efficiency and quality.

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

[0070] Specifically, step S240 uses a dynamic window method to perform local path optimization on the globally optimized planned path to obtain the globally and locally optimized planned paths, including the following steps:

[0071] Extract key nodes based on the globally optimized planned path to obtain multiple key nodes;

[0072] Determine a dynamic window, the dynamic window including a velocity constraint range, and simulate each local path for a period of time multiple times using a kinematic model with each key node as a starting point based on each set of velocities within the velocity constraint range to obtain multiple candidate local trajectories corresponding to each local path, wherein the velocities include linear velocity and angular velocity;

[0073] 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 globally and locally optimized planned path.

[0074] In the above steps, key nodes are nodes that are extracted from the global planning path after global optimization and have a decisive effect on local path optimization, such as turning points for bypassing obstacles, path segmentation nodes, and dynamic adjustment trigger nodes. The speed constraint range V1 can be the speed range limited by the flying car's own constraints, as shown in the following formula:

[0075] V1={(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max ]}

[0076] Among them, v min and v max Respectively represent the minimum linear velocity and maximum linear velocity of the flying car, ω min and ω maxRespectively represent the minimum angular velocity and maximum angular velocity of the set flying car. The above speed range is used as a dynamic window, and the dynamic window is sampled to obtain multiple sets of speeds for simulating local paths. For each set of speeds, the speed of each set of speeds is used as the speed at each key node. The flying car can fly at this speed to simulate or predict each local path or trajectory for a period of time Δt in the future with each key node as the starting point, and finally obtain multiple candidate local trajectories corresponding to each local path through flight simulation. The increase or decrease of the speed V2 after a period of time or predicted time Δt can be limited by the following formula:

[0077] V2={(v,ω)|v∈[v t -a·Δt,v t +a·Δt],ω∈[ω t -b·Δt,ω t +b·Δt]}

[0078] Among them, v t and ω t They represent the linear velocity and angular velocity in each group of velocities, a represents linear acceleration, and b represents angular acceleration.

[0079] The evaluation function represents the weighted sum of the cost functions, and its purpose is to select the optimal local trajectory from all candidate local trajectories of each local path, thereby optimizing each local path. The expression of the evaluation function can be:

[0080] COST(v,ω)=α·Head(v,ω)+β·dist(v,ω)+γ·Vel(v,ω)

[0081] Here, Head(v,ω) represents the angular deviation between the end of the candidate local trajectory and the target node, dist(v,ω) represents the distance between the candidate local trajectory and the nearest obstacle, Vel(v,ω) represents the simulated velocity, and α, β, and γ represent weight coefficients. The smaller Head(v,ω), dist(v,ω), and Vel(v,ω), that is, the smaller COST(v,ω), the better the corresponding candidate local trajectory. The purpose of the cost function Head(v,ω) is to make the flying car fly towards the target node, the purpose of the cost function dist(v,ω) is to avoid collisions between the flying car and obstacles (the farther the distance between the trajectory and the nearest obstacle, the smaller the dist(v,ω) value). The purpose of the cost function Vel(v,ω) is to make the distance between the node reached by the flying car and the target node closer within the set time Δt (the greater the simulated velocity, the smaller the Vel(v,ω) value).

[0082] In the above steps, by integrating motion constraints in the local path optimization process and introducing restrictions on the dynamic parameters of flying cars (such as linear velocity, angular velocity, acceleration, etc.), the physical feasibility of the trajectory is achieved, which enhances the engineering practicality of the algorithm.

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

[0084] Smoothing the globally optimized planned path to obtain the globally optimized planned path trajectory;

[0085] Correspondingly, the dynamic window method is used to perform local path optimization on the globally optimized planned path trajectory to obtain the globally and locally optimized planned path trajectory.

[0086] Specifically, B-spline curves can be used for smoothing, which can solve the problem that the planned path obtained by the traditional A* algorithm is not smooth.

[0087] Corresponding to the above method, the present invention also provides a ground 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.

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

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

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

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

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

[0093] 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 ground path planning method, characterized in that: The method comprises: Determine a two-dimensional map based on the ground environment of the target planning area, wherein the two-dimensional map includes a starting node, a target node, and obstacles; An improved A-star algorithm is used to sequentially search for intermediate nodes between the starting node and the target node to obtain a planned path. The intermediate nodes are determined by estimating the cost of each non-obstruction node based on the adjacent non-obstruction nodes of the previous node and a heuristic function based on dynamic weights. Determining whether every three consecutive nodes in the planned path along the direction from the start node to the target node are collinear or whether a line between two nodes other than the middle node among the three nodes does not pass through an obstacle to globally optimize the planned path, thereby obtaining a globally optimized planned path; The dynamic window method is used to perform local path optimization on the globally optimized planning path to obtain the globally and locally optimized planning paths.

2. The method according to claim 1, characterized in that The non-obstacle nodes adjacent to the previous node include: the angle between the straight line between the previous node and the target node and the positive x-axis is in the range of 0° to 90°, 90° to 180°, 180° to 270° or 270° to 360°, and the corresponding non-obstacle nodes in the area facing the target node adjacent to the previous node.

3. The method according to claim 1, characterized in that The cost of the non-obstruction node includes the movement cost from the starting node to the non-obstruction node and the estimated movement cost from the non-obstruction node to the target node with a dynamic weight, wherein the farther the non-obstruction node is from the target node, the greater the weight of the estimated movement cost, and the closer the non-obstruction node is to the target node, the smaller the weight of the estimated movement cost, until it drops to 0.

4. The method according to claim 3, characterized in that The calculation formula of the cost of the non-obstacle node is as follows: f(n)=g(n)+w(n)h(n) Where f(n) represents the cost of the non-obstruction node, g(n) represents the movement cost from the starting node to the non-obstruction node, h(n) represents the estimated movement cost from the non-obstruction node to the target node, w(n) represents the dynamic weight, D represents the cost from the non-obstruction node to the target node, and T represents the cost from the starting node to the target node.

5. The method according to claim 4, characterized in that The moving cost from the starting node to the non-obstacle node is obtained by calculating the Euclidean distance between the starting node and the non-obstacle node, and the estimated moving cost from the non-obstacle node to the target node is obtained by calculating the Manhattan distance between the non-obstacle node and the target node.

6. The method according to claim 1, characterized in that Determining whether every three consecutive nodes in the planned path along the direction from the start node to the target node are collinear or whether a line between two nodes other than the middle node among the three nodes does not pass through an obstacle to globally optimize the planned path, and obtaining a globally optimized planned path, includes: If every three consecutive nodes in the planned path along the direction from the start node to the target node are collinear or the line between two nodes other than the middle node among the three nodes does not pass through an obstacle, then each middle node is determined to be a redundant node and the redundant node is deleted to obtain a globally optimized planned path; The method of using the dynamic window method to perform local path optimization on the globally optimized planned path to obtain the globally and locally optimized planned paths includes: Extract key nodes based on the globally optimized planned path to obtain multiple key nodes; Determine a dynamic window, the dynamic window including a velocity constraint range, and simulate each local path for a period of time multiple times using a kinematic model with each key node as a starting point based on each set of velocities within the velocity constraint range to obtain multiple candidate local trajectories corresponding to each local path, wherein the velocities include linear velocity and angular velocity; 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 globally and locally optimized planned path.

7. The method according to any one of claims 1 to 6, characterized in that Before the step of performing local path optimization on the globally optimized planned path using the dynamic window method, the method further includes: Smoothing the globally optimized planned path to obtain the globally optimized planned path trajectory; Correspondingly, the dynamic window method is used to perform local path optimization on the globally optimized planned path trajectory to obtain the globally and locally optimized planned path trajectory.

8. A ground 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