Four-dimensional trajectory planning method for UAV swarms in high-density operation scenarios

Abstract

The present invention relates to the technical field of unmanned aerial vehicle (UAV) trajectory planning, and discloses a four-dimensional trajectory planning method for a UAV cluster facing a high-density operation scenario, comprising the steps of: constructing a four-dimensional grid model of urban airspace to complete the reconstruction of the four-dimensional spatiotemporal data of the UAV; the four-dimensional grid model of urban airspace includes a fixed four-dimensional body model, a four-dimensional point model of the UAV, and a four-dimensional line model of the trajectory; constructing a four-dimensional trajectory planning model for the UAV; improving an adaptive heuristic function based on a 4D-A* search algorithm; and obtaining the optimal trajectory of the UAV based on the operating cost and operating risk obtained by the improved adaptive heuristic function. The present invention can effectively complete the conflict-free real-time four-dimensional trajectory planning of the high-density operation of the urban UAV cluster, improve the operating efficiency and reduce the operating risk.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle (UAV) trajectory planning, and in particular to a four-dimensional trajectory planning method for an unmanned aerial vehicle (UAV) cluster facing high-density operation scenarios. Background Art

[0002] In recent years, with the rapid development of drone technology, drones have been widely used in various fields. In order to ensure that drones can complete tasks autonomously and smoothly in cluster cities, it depends on reasonable mission path planning for the fleet, so that it can improve the efficiency of mission completion while ensuring safe flight. Drone path planning refers to finding a flight path for a drone to fly from the starting point to the target point in a certain environment and avoid all obstacles in the way through algorithms, control, etc. Since multiple drones of different types perform tasks in the same airspace in the city, the low-altitude airspace in the city will become crowded and complex, which will increase the difficulty of multi-drone planning before flight and the risk of collision between drones. Faced with the problem of multi-drone mission planning in the context of complex urban airspace, a more efficient drone collaborative planning algorithm is needed.

[0003] Currently, a variety of mature path planning algorithms have been applied, including sampling-based algorithms such as rapidly exploring random numbers (RRT) and probabilistic roadmaps (PRM), search-based algorithms such as Dijkstra's algorithm and A* search algorithm, as well as potential field methods, optimal control methods, and population-based algorithms. In path planning, the time dimension is not fully considered, resulting in the decoupling of time and space dimension attributes, and the planned path is not optimal in complex urban scenarios. Summary of the invention

[0004] The purpose of the present invention is to complete the conflict-free real-time four-dimensional trajectory planning of urban drone clusters in high-density operation, improve the operation efficiency and reduce the operation risk, and provide a four-dimensional trajectory planning method for drone clusters in high-density operation scenarios.

[0005] In order to achieve the above-mentioned object of the invention, the embodiment of the present invention provides the following technical solutions:

[0006] The four-dimensional trajectory planning method of UAV swarm for high-density operation scenarios includes the following steps:

[0007] Step 1, constructing a four-dimensional grid model of urban airspace to complete the reconstruction of the four-dimensional spatiotemporal data of the UAV; the four-dimensional grid model of urban airspace includes a fixed four-dimensional volume model, a four-dimensional point model of the UAV, and a four-dimensional line model of the track;

[0008] Step 2: construct a four-dimensional trajectory planning model for the UAV;

[0009] Step 3, improving the adaptive heuristic function based on the 4D-A* search algorithm;

[0010] Step 4: Based on the operation cost and operation risk obtained by improving the adaptive heuristic function, the optimal trajectory of the UAV is obtained.

[0011] Compared with the prior art, the beneficial effects of the present invention are as follows: the drone cluster of the present invention can simultaneously plan the time-space conflict-free trajectories of several flight plans in a simulation test environment, and can effectively complete the conflict-free real-time four-dimensional trajectory planning of high-density operation of urban drone clusters, thereby improving operation efficiency and reducing operation risks. BRIEF DESCRIPTION OF THE DRAWINGS

[0012] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0013] Figure 1 is a flow chart of the method of the present invention;

[0014] Figure 2 It is a schematic diagram of the four-dimensional grid model of the urban airspace of the present invention. DETAILED DESCRIPTION

[0015] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. The components of the embodiments of the present invention generally described and shown in the drawings here can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the claimed invention, but merely represents the selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without making creative work belong to the scope of protection of the present invention.

[0016] It should be noted that similar numbers and letters represent similar items in the following figures, so once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures. At the same time, in the description of the present invention, the terms "first", "second", etc. are only used to distinguish the description, and cannot be understood as indicating or implying relative importance, or implying any such actual relationship or order between these entities or operations. In addition, the terms "connected", "connected", etc. can be directly connected between elements, or indirectly connected via other elements.

[0017] The present invention is achieved through the following technical solutions: Figure 1 As shown, a four-dimensional trajectory planning method for a UAV cluster in a high-density operation scenario includes the following steps:

[0018] Step 1: Build a four-dimensional grid model of the urban airspace and complete the reconstruction of the four-dimensional spatiotemporal data of the drone.

[0019] Based on the concept of discrete grids, the urban airspace is modeled as a four-dimensional space-time block. On the basis of the new grid hierarchy architecture, targets with different characteristics in the urban airspace are digitally reconstructed, including: fixed four-dimensional volume model, UAV four-dimensional point model, and track four-dimensional line model.

[0020] The difference between the two-dimensional and three-dimensional grids is that the four-dimensional grid model of urban airspace adopts a four-dimensional voxel world, which is a four-dimensional voxel unit V with time information. i The four-dimensional voxel unit V i It is expressed as:

[0021]

[0022] Among them, {x i ,y i ,z i ,t i} represents the four-dimensional voxel unit V i The space-time coordinates, l i ={x i ,y i ,z i} represents the four-dimensional voxel unit V i The spatial coordinates, x i Represents the horizontal axis, y i Represents the vertical coordinate, z i Indicates the vertical coordinate, t i Represents the time coordinate, i=1,2,...,M, M represents the number of four-dimensional voxel units in the four-dimensional voxel world.

[0023] Fixed 4D Volume Model of 4D Grid Model of Urban Airspace Build as:

[0024]

[0025] Among them, X, Y, Z, and T represent x, i ,y i 、z i ,t i Complete works of .

[0026] For fixed spaces or objects in urban airspace scenarios (such as buildings, no-fly zones, assigned tracks, etc.), they can be represented by an unordered set of occupied airspace Volume:

[0027]

[0028] Among them, V j Represents a fixed 4D volume model The occupied four-dimensional voxel unit in, j=1,2,...,m, m represents a fixed four-dimensional volume model The number of occupied 4D voxel units in .

[0029] Since the urban airspace four-dimensional grid model has digitally reconstructed the four-dimensional space-time information of the urban airspace at multiple scales, and the digitally reconstructed grid is unique, drones appearing at any time and at any place can establish a logical mapping with the urban airspace four-dimensional grid model to complete the reconstruction of the drone's four-dimensional space-time data. Therefore, in the urban airspace four-dimensional grid model, the drone four-dimensional point model can be expressed as:

[0030]

[0031] Among them, Grid UAV Represents the four-dimensional point model of a drone appearing at any location at any time, V k It means that the drone appears at the kth four-dimensional voxel unit at any time, k=1,2,...,M.

[0032] Given the starting position and target position of the drone, the present invention uses an efficient four-dimensional search algorithm to plan a safe and effective path. According to the aforementioned construction method of the fixed four-dimensional volume model and the four-dimensional point model of the drone, the drone's track can be discretized into a series of heterogeneous networks. At this time, the drone's track four-dimensional line model can be represented by an ordered set Path composed of n+1 four-dimensional voxel units:

[0033]

[0034] Among them, Path represents the four-dimensional line model of the track, V p represents the pth four-dimensional voxel unit occupied by the drone's track, p=0,1,...,n; n+1 represents the number of four-dimensional voxel units occupied by the drone's track, V0 represents the four-dimensional voxel unit where the starting position is located, V n The 4D voxel unit representing the target location.

[0035] The above completes the fixed four-dimensional model , UAV 4D point model Grid UAV , construction of the four-dimensional line model Path.

[0036] After step 1, the method further includes the step of dividing the urban airspace altitude layers.

[0037] Normally, the approach and departure phases of drones are directly controlled by ground airports, while the cruising phase is only carried out in a certain altitude range. In urban airspace below 150m, the use of altitude layer division can enable drones to choose shorter and straighter paths during collaborative planning, greatly reducing the probability of conflict and effectively improving the flexibility of drones during flight.

[0038] Taking into account factors such as the scale of drone swarms, the safe distance between drones, and the requirements for drone swarm collaborative tasks, the urban airspace below 150m is divided into three altitude layers: 40-80m, 80-115m, and 115-150m. The upper limit of each altitude layer is used as the projection surface, and the drones are abstracted as particles, and two-dimensional path planning is performed on the projection surface. The low-altitude layer of 40-80m is suitable for micro drones; the medium-altitude layer of 80-115m is suitable for medium-sized drones; and the high-altitude layer of 115-150m is suitable for large drones.

[0039] Step 2: Build a four-dimensional trajectory planning model for the UAV.

[0040] This step establishes a mathematical model for the graph-based path planning problem and presents a four-dimensional trajectory planning model for UAVs based on the 4D-A* search algorithm under a unified spatiotemporal map model.

[0041] According to the fixed four-dimensional model The four-dimensional grid state space is and Respectively represent the starting node and the target node in the flight track. is an unordered set of occupied airspaces, the flight path planning problem of the UAV can be regarded as finding a four-dimensional path Path that minimizes the operation cost G(Path) of the entire path and has no collision. end The spatial coordinates of are determined, namely l end ={x end ,y end ,z end}, and the time coordinate t end is uncertain, where x end ,y end 、z end Represents the V of the target node respectively end The horizontal coordinate, vertical coordinate and vertical coordinate of the path are assuming that the path is composed of an ordered set of n+1 four-dimensional voxel units, then the four-dimensional trajectory planning model can be expressed as:

[0042]

[0043]

[0044]

[0045] Among them, indicates that the track Path and the set of occupied voxel units Volume are an empty set, representing no collision conflict in this track planning; represents the running cost of the track Path, including spatial distance cost and time cost, and is specifically expressed as:

[0046] When p = 1, Cost(V p ) = |l0, l1| + t1 - t0;

[0047] When 2 < p < n, Cost(V p ) = 1 / 2(|l p , l p-1 | + |l p , l p+1 |) + t p - t p-1 ;

[0048] When p = n, Cost(V p ) = |l n , l n-1 | + t n - t n-1 ;

[0049] And there are l0 = l start , l n = l end ;

[0050] Among them, l0 and l start both represent the spatial coordinates of the initial node, l n and l end both represent the spatial coordinates of the target node, t0 represents the time coordinate of the UAV at l0, t n represents the time coordinate of the UAV at l end , p = 0, 1,..., n; |l p , l p-1 | represents the Euclidean distance in three-dimensional space between the spatial coordinate l p and the spatial coordinate l p-1 ; t p represents the time coordinate of the UAV at l p , t p-1 represents the time coordinate of the UAV at l p-1 .

[0051] represents the running risk of each four-dimensional voxel unit in the track, and is specifically expressed as:

[0052]

[0053] Among them, a1 is the adjustment coefficient of operational risk attention, which is a positive real number. If a1 is larger, more attention will be paid to operational risk during route planning, and vice versa, more attention will be paid to operational cost; a2 represents the adjustment coefficient of operational risk sensitivity, which is a positive real number. When a2 is larger, the occupied four-dimensional voxel unit V at a longer distance will be min It will also significantly increase the operating risk. On the contrary, only when V min Only when it is advanced enough will the operating risk increase significantly; e represents the base of the natural logarithm function.

[0054]

[0055] Among them, d min Indicates V p The minimum Euclidean distance in four-dimensional space to all four-dimensional voxel units in Volume, Represents the four-dimensional voxel unit occupied by the minimum Euclidean distance, Respectively represent V min The horizontal coordinate, vertical coordinate, ordinate and time coordinate.

[0056] like Figure 2 The figure shows a schematic diagram of a four-dimensional grid model of urban airspace, where each grid represents a four-dimensional voxel unit V i , the horizontal axis represents x i , the ordinate represents y i , the vertical axis represents z i , which is equivalent to the spatial coordinate system. The red four-dimensional voxel units represent the four-dimensional voxel units occupied by the path of a drone. The lighter the red, the shorter the time distance, and the darker the red, the longer the time distance; the green four-dimensional voxel units represent the four-dimensional voxel units occupied by the path of another drone. The lighter the green, the shorter the time distance, and the darker the green, the longer the time distance.

[0057] Step 3: Improve the adaptive heuristic function based on the 4D-A* search algorithm.

[0058] The 4D-A* search algorithm introduces the time dimension and operation risk based on the three-dimensional grid search. p The closer to the target node V end , its spatial distance cost will become smaller and smaller, but the time cost will increase accordingly. Therefore, this step improves the adaptive heuristic function based on the 4D-A* search algorithm and adjusts the function size through the adaptive factor, specifically:

[0059]

[0060] Among them, H(V p ,l end ) represents the slave node V p To the target location end The estimated cost of is the weight factor for estimating operating costs, To estimate the weight factor of operational risk, the weight factor is adjusted according to the needs of specific application scenarios to balance the importance of space and time.

[0061] H Cost (V p ,l end ) represents the current node V p ={l p ,t p} to the target position l end The estimated operating cost of a track is generally proportional to the spatial distance of the track, specifically:

[0062]

[0063] Where, e represents the base of the natural logarithm function; represents the first adaptive adjustment factor, which can be obtained according to the actual operating cost of the assigned tracks in the environment. Suppose the set of all assigned tracks is {Path b |b=1,2,...,L}, L represents the number of tracks, then It can be obtained by the following expression:

[0064]

[0065] Among them, C (Path b ) represents the bth track Path b The actual operating cost, D(Path b ) represents the bth track Path b The Euclidean distance between the starting position and the target position in space. If in the bth track Path b In the unassigned initial state, take .

[0066] H Risk (V p ,l end ) represents the current node V p ={l p ,t p} to the target position l end The estimated operation risk of Path H The current node V p Arrival at target locationend The linear four-dimensional path of , the specific estimated operation risk is:

[0067]

[0068] Among them, sum(.) means to find the number of elements in a set; e means the base of the natural logarithm function; represents the second adaptive adjustment factor, which can be obtained according to the actual operating cost of the assigned tracks in the environment. Suppose the set of all assigned tracks is {Path b |b=1,2,...,L}, L represents the number of tracks, then It can be obtained by the following expression:

[0069]

[0070] Among them, R (Path b ) represents the bth track Path b The actual operational risk; if in the bth track Path b In the unassigned initial state, take .

[0071] Step 4: Based on the operation cost and operation risk obtained by improving the adaptive heuristic function, the optimal trajectory of the UAV is obtained.

[0072] The step 4 specifically comprises the following steps:

[0073] Step 4-1, Initialization: Set the starting node V start Add to the open list and set the actual cost of the current track G[Path(V p ,V start )] is 0, which means V p =V start , Path(V p ,V start ) indicates that the initial node V p To the starting node V start The actual route of H(V p ,l end ) represents the initial node V p To the target location end The estimated cost of calculating the F value:

[0074]

[0075] Step 4-2, check whether the open list is empty: if the open list is empty, the algorithm ends and no track path is found; if the open list is not empty, select the V corresponding to the minimum F value from the open list p As the current node, and the Vp Move to Close List;

[0076] Step 4-3, determine the current node V p Has the target location been reached? end :If it has been reached, then backtrack to the target node V end To the starting node V start If the path is not reached, the algorithm ends; if not, continue to step 4-4.

[0077] Step 4-4, for the current node V p Each adjacent node V adjacent Do the following:

[0078] 1. If the adjacent node V adjacent If it is already in the closed list, skip this adjacent node V adjacent ;

[0079] 2. Calculate or update adjacent nodes V adjacent G[Path(V adjacent ,V start )] value, G[Path(V adjacent ,V start )] represents the node V adjacent To the starting node V start the actual cost of

[0080] 3. Calculate the adjacent node V adjacent H(V adjacent ,l end ) value, H(V adjacent ,l end ) represents the node V from the adjacent node adjacent To the target location end The estimated cost of

[0081] 4. Calculate the adjacent node V adjacent The F value F=G[Path(V adjacent ,V start )]+H(V adjacent ,l end );

[0082] 5. If the adjacent node V adjacent If it is not in the open list, add it to the open list and set its parent node to the current node V p , the parent node represents the adjacent node V adjacent The previous node of

[0083] 6. If the adjacent node V adjacent Already in the open list, but through the current node V pThe path to reach is better (that is, the new G[Path(V adjacent ,V start )] value is smaller), then update its G[Path(V adjacent ,V start )], F value and parent node.

[0084] Step 4-5, return to step 4-2 until the target position l is found end Or the open list is empty.

[0085] Through step 4, the minimum value of the sum of the estimated operating cost and the expected operating risk is continuously searched and iterated to obtain the optimal trajectory path planning of the UAV.

[0086] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A four-dimensional trajectory planning method for UAV swarms in high-density operation scenarios, characterized by: The following steps are involved: Step 1, constructing a four-dimensional grid model of urban airspace to complete the reconstruction of the four-dimensional spatiotemporal data of the UAV; the four-dimensional grid model of urban airspace includes a fixed four-dimensional volume model, a four-dimensional point model of the UAV, and a four-dimensional line model of the track; In step 1, the fixed four-dimensional volume model is: Among them, V i represents the i-th four-dimensional voxel unit in the fixed four-dimensional volume model, i=1,2,...,M, M is the number of four-dimensional voxel units; it has: Among them, {x i ,y i ,z i ,t i } represents the four-dimensional voxel unit V i The space-time coordinates, l i ={x i ,y i ,z i } represents the four-dimensional voxel unit V i The spatial coordinates, x i Represents the horizontal axis, y i Indicates the vertical coordinate, z i Indicates the vertical coordinate, t i Indicates the time coordinate; X, Y, Z, T represent x i ,y i 、z i ,t i Complete works of; For fixed spaces or objects in urban airspace scenes, an occupied airspace unordered set Volume is used to represent them: Among them, V j Represents a fixed 4D volume model The occupied four-dimensional voxel unit in, j=1,2,...,m, m represents a fixed four-dimensional volume model The number of occupied 4D voxel units in ; In step 1, the track four-dimensional line model is: Among them, Path represents the four-dimensional line model of the track, V p represents the pth four-dimensional voxel unit occupied by the drone's track, p=0,1,...,n; n+1 represents the number of four-dimensional voxel units occupied by the drone's track, V0 represents the four-dimensional voxel unit where the starting position is located, V n The four-dimensional voxel unit representing the target location; Step 2: construct a four-dimensional trajectory planning model for the UAV; The step 2 specifically includes the following steps: use and Respectively represent the starting node and the target node in the flight path; assuming that the path Path is composed of an ordered set of n+1 four-dimensional voxel units, the four-dimensional trajectory planning model is expressed as: Among them, G(Path) represents the running cost of the track Path; It represents the running cost of the track Path, including the spatial distance cost and the time cost, which is specifically expressed as: When p=1, Cost(V p )=|l0,l1|+t1-t0; When 2 < p < n, Cost(V p ) = 1 / 2(|l p , l p-1 | + |l p , l p+1 |) + t p - t p-1 ; When p=n, Cost(V p )=|l n ,l n-1 |+t n -t n-1 ; And l0=l start , l n =l end ; Among them, l0 and l start Both represent the spatial coordinates of the initial node, l n and l end Both represent the spatial coordinates of the target node, t0 represents the time coordinate of the drone at l0, and t n Indicates that the drone is in l end The time coordinate of p=0,1,...,n; |l p ,l p-1 | represents the spatial coordinate l p and spatial coordinates l p-1 Euclidean distance in three-dimensional space; t p Indicates that the drone is in l p The time coordinate, t p-1 Indicates that the drone is in l p-1 The time coordinate of It represents the operational risk of each four-dimensional voxel unit in the track, which is specifically expressed as: Among them, a1 is the adjustment coefficient of operation risk attention; a2 is the adjustment coefficient of operation risk sensitivity; e is the base of the natural logarithm function; d min Indicates V p The minimum Euclidean distance in four-dimensional space to all four-dimensional voxel units in Volume is: in, Represents the four-dimensional voxel unit occupied by the minimum Euclidean distance, Respectively represent V min The horizontal coordinate, vertical coordinate, vertical coordinate and time coordinate of Step 3, improving the adaptive heuristic function based on the 4D-A* search algorithm; Step 4: Based on the operation cost and operation risk obtained by improving the adaptive heuristic function, the optimal trajectory of the UAV is obtained.

2. The four-dimensional trajectory planning method for UAV swarms in high-density operation scenarios according to claim 1 is characterized by: In step 1, the four-dimensional point model of the drone is: Among them, Grid UAV Represents the four-dimensional point model of a drone appearing at any location at any time, V k It means that the drone appears at the kth four-dimensional voxel unit at any time, k=1,2,...,M, and M is the number of four-dimensional voxel units.

3. The four-dimensional trajectory planning method for UAV swarms in high-density operation scenarios according to claim 1 is characterized by: The step 3 specifically comprises the following steps: Adaptive heuristic function, and adjust the function size through adaptive factors, specifically: Among them, H(V p ,l end ) represents the slave node V p To the target location end the estimated cost of is the weight factor for estimating operating costs, is the weight factor for estimating operational risk; H Cost (V p ,l end ) represents the current node V p ={l p ,t p } to the target position l end The estimated operating costs are: Where, e represents the base of the natural logarithm function; represents the first adaptive adjustment factor; H Risk (V p ,l end ) represents the current node V p ={l p ,t p } to the target position l end The estimated operational risks are: Among them, Path H Indicates that the current node V p Arrival at target location end The straight line four-dimensional path of ; sum(.) means to find the number of elements in the set; e means the base of the natural logarithm function; Represents the second adaptive adjustment factor.

4. The four-dimensional trajectory planning method for UAV swarms in high-density operation scenarios according to claim 3 is characterized by: Suppose the set of all assigned tracks is {Path b |b=1,2,...,L}, L represents the number of tracks, then It is obtained by the following expression: Among them, C (Path b ) represents the bth track Path b The actual operating cost, D(Path b ) represents the bth track Path b The Euclidean distance between the starting position and the target position in space. If in the bth track Path b In the unassigned initial state, take .

5. The four-dimensional trajectory planning method for UAV swarms in high-density operation scenarios according to claim 3 is characterized by: Suppose the set of all assigned tracks is {Path b |b=1,2,...,L}, L represents the number of tracks, then It is obtained by the following expression: Among them, R (Path b ) represents the bth track Path b The actual operational risk; if in the bth track Path b In the unassigned initial state, take .

6. The four-dimensional trajectory planning method for UAV swarms in high-density operation scenarios according to claim 3 is characterized by: The step 4 specifically comprises the following steps: Step 4-1, Initialization: Set the starting node V start Add to the open list and set the actual cost of the current track G[Path(V p ,V start )] is 0, which means V p =V start , Path(V p ,V start ) indicates that the initial node V p To the starting node V start The actual route of H(V p ,l end ) represents the initial node V p To the target location end The estimated cost of calculating the F value: Step 4-2, check whether the open list is empty: if the open list is empty, the algorithm ends and no track path is found; if the open list is not empty, select the V corresponding to the minimum F value from the open list p As the current node, and the V p Move to Close List; Step 4-3, determine the current node V p Has the target location been reached? end :If it has been reached, then backtrack to the target node V end To the starting node V start The algorithm ends if the path is not reached. If not, continue to step 4-4. Step 4-4, for the current node V p Each adjacent node V adjacent Do the following:

1. If the adjacent node V adjacent If it is already in the closed list, skip this adjacent node V adjacent ; 2. Calculate or update adjacent nodes V adjacent G[Path(V adjacent ,V start )] value, G[Path(V adjacent ,V start )] represents the node V adjacent To the starting node V start the actual cost of 3. Calculate the adjacent node V adjacent H(V adjacent ,l end ) value, H(V adjacent ,l end ) represents the node V from the adjacent node adjacent To the target location end The estimated cost of 4. Calculate the adjacent node V adjacent The F value F=G[Path(V adjacent ,V start )]+H(V adjacent ,l end ); 5. If the adjacent node V adjacent If it is not in the open list, add it to the open list and set its parent node to the current node V p , the parent node represents the adjacent node V adjacent The previous node of 6. If the adjacent node V adjacent Already in the open list, but through the current node V p The path to reach is better (that is, the new G[Path(V adjacent ,V start )] value is smaller), then update its G[Path(V adjacent ,V start )], F value and parent node; Step 4-5, return to step 4-2 until the target position l is found end Or the open list is empty.

Images