CN114740898B - A three-dimensional trajectory planning method based on free space and A* algorithm - Google Patents

Abstract

The present invention discloses a three-dimensional trajectory planning method based on free space and A* algorithm. First, according to the given planning space height dimension, the mission flight surface is extracted based on the plane binary map, and the path planning problem of the three-dimensional map is converted into the path planning problem of the two-dimensional map, thereby reducing the complexity of the path planning. Afterwards, the binary image connected domain detection algorithm is used to identify and distinguish the various obstacle areas in the mission flight surface, and the obstacle rectangle segmentation and merging method is used to convert the obstacles into polygons. Finally, an improved A* algorithm based on graphic preprocessing is used to find the optimal path. Compared with the existing method, the present invention has the advantages of high planning efficiency, high flyability and high traceability of the planned trajectory.

Description

A three-dimensional trajectory planning method based on free space and A* algorithm

Technical Field

The invention relates to the field of track planning, and in particular to a track planning method based on graphics and heuristic search, and in particular to a three-dimensional track planning method based on free space and A* algorithm.

Background technique

Drones are being widely used in military and civilian fields, including battlefield reconnaissance, disaster monitoring, and environmental monitoring, due to their small size, low cost, and high flexibility. To achieve these functions, the trajectory planning function of drones is essential. The trajectory planning of drones is to follow certain constraints and criteria in a known or unknown working environment and autonomously find an optimal barrier-free route from the starting position to the target position. Since the complexity of three-dimensional maps is higher than that of two-dimensional maps, the trajectory planning algorithm of three-dimensional maps is often more complex and more time-consuming. The A* algorithm is a classic trajectory planning algorithm that can achieve better trajectory planning, but as the map becomes more complex, the computational complexity and search redundancy of the A* algorithm increase exponentially, resulting in low efficiency in searching for trajectories, and the searched trajectories are not optimal.

Based on the A* algorithm, there are many improvements to the A* algorithm, including modifying the heuristic function in the A* algorithm, changing the method of finding nodes, increasing the neighborhood search range, etc. These methods avoid redundant searches to a certain extent and improve the track search efficiency and track smoothness of the A* algorithm. However, these improved A* algorithms still have problems such as slow planning speed and non-optimal tracks.

Summary of the invention

In order to solve the complex problem of three-dimensional trajectory planning, the present invention proposes a three-dimensional trajectory planning method based on free space and A* algorithm.

The present invention is based on the three-dimensional trajectory planning method of free space and A* algorithm, and the specific steps are designed as follows:

Step 1: According to the given planning space height dimension, all nodes of the corresponding height are extracted from the terrain elevation model to form the mission flight surface.

Step 2: Use the binary image connected domain detection algorithm to identify and distinguish the various obstacle areas in the mission flight surface, and further convert the obstacles on the mission flight surface into polygons.

Step 3: The free space on the mission flight surface can be regarded as a multiply connected concave polygon. First, the multiply connected concave polygon is converted into a simply connected concave polygon; then the simply connected concave polygon is decomposed into several sub-convex polygons.

Step 4: Use the A* algorithm based on graph preprocessing to find the optimal trajectory.

First, each sub-free space on the mission flight surface is processed into a node used by the A* algorithm; the specific method is as follows:

Add node number, node coordinates, adjacent node number, parent node number, and heuristic information to the child free space.

Then, the A* algorithm is used to search for the optimal region channel.

After obtaining the optimal regional channel, an initial feasible track connecting the starting point and the end point is generated in the optimal regional channel; the obtained track is further optimized; the optimization method is:

① Let the number of global searches i = 0;

② Let the number of local searches j = 0;

③ Take the midpoints {M ₁ ,M ₂ ,…M _n } on the common dividing line of each pair of adjacent _areas in the regional channel, and connect the initial feasible track {X _current }:X _start →M ₁ →M 2 ...→M _{n →X end through the midpoints {M 1 ,M 2 , … M n} };

④ Randomly select a segmentation line Q _k Q _k+1 and adjust the track node M _k →R _k on it. R _k is another point on the segmentation line. After reconnecting, a new track is obtained:

{X _new }:X _start →M ₁ →M ₂ ... →R _k → ... →M _n →X _end

and calculate its length;

⑤ If the new track {X _new } is shorter than the current track {X _current }, replace the current track with the new track: {X _current }←{X _new }, let j=0; if the new track is longer than the current track, let j=j+1;

⑥ Repeat steps ④ and ⑤ until j is greater than the local search upper limit t, and let i = i + 1;

⑦ Repeat steps ③ to ⑥ until i is greater than the global search upper limit T.

The three-dimensional trajectory planning method based on free space and A* algorithm of the present invention has the advantages of high planning efficiency, high flyability and high trackability of the planned trajectory compared with the prior art.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a flow chart of a three-dimensional trajectory planning method based on free space and A* algorithm of the present invention;

FIG2 is a schematic diagram of the results of extracting the mission flight surface according to the present invention;

FIG3 is a schematic diagram of obstacle identification in the present invention;

FIG4 is a schematic diagram of an obstacle conversion polygon in the present invention;

FIG5 is a schematic diagram of constructing a simply connected polygon in the present invention;

FIG6 is a schematic diagram of a map segmentation result in the present invention;

FIG7 is a schematic diagram of the optimal visible point in the present invention;

FIG8 is a schematic diagram of a two-dimensional trajectory planning result in the present invention;

FIG9 is a schematic diagram of the flight surface trajectory planning result in the present invention;

FIG. 10 is a schematic diagram of the three-dimensional trajectory planning result in the present invention.

Detailed ways

The present invention will be described in further detail below in conjunction with the accompanying drawings.

The three-dimensional trajectory planning method based on free space and A* algorithm of the present invention is shown in FIG1 , and the specific steps are designed as follows:

Step 1: According to the given planning space height dimension, all nodes of corresponding height are extracted from the terrain elevation model to form the mission flight surface;

In order to reduce the complexity of trajectory planning, the mission flight surface is extracted based on the plane binary map, and the three-dimensional trajectory planning problem is reduced to a two-dimensional trajectory planning problem. The specific method is:

Traverse all nodes in the terrain elevation model. If the terrain height _hg corresponding to node A satisfies _{hg ≤hmax} - _hm , node A is regarded as a safe node on the mission flight surface, and the corresponding node of node A on the binary map is 0; otherwise, node A is regarded as an obstacle node, and the corresponding node of node A on the binary map is 1. The above _hmax represents the maximum flight sea height of the UAV; _hm represents the relative ground height specified by the mission. The mission flight surface extracted by the above method is shown in Figure 2.

Step 2: Convert obstacles on the mission flight surface into polygons

In order to use the A* algorithm based on graphic preprocessing on the binary map, it is necessary to convert the obstacles on the mission flight surface into polygons. To this end, it is first necessary to use the binary image connected domain detection algorithm to identify and distinguish the various obstacle areas on the mission flight surface, as shown in Figure 3. Let the binary map matrix have m rows and n columns, and the specific process of obstacle area identification is:

1) Visit node x ₁₁ . If it is an obstacle node, the class order of the node is recorded as 1.

2) Traverse node _x1j (j=2,...,n). If it is an obstacle node, check whether its left node x1 _(j-1) is an obstacle node. If so, _x1j uses the class sequence of x1 _(j-1) . Otherwise, the class sequence of _x1j is the current total number of classes (the total number of obstacle node class sequences) plus 1. For example, suppose there are 10 points in the first row, namely _x11 , _x12 , _x13 ,..., _x110 , among which _x11 , _x13 , _x15 , and _x16 are obstacle points. The class sequence of _x11 is 1; point _x12 is a non-obstacle point and has no class sequence; _x13 is an obstacle point, so its left point _x12 is detected to see if it is an obstacle point, and the result is no, so the class sequence of _x13 is 1+1=2; _x14 is a non-obstacle point and has no class sequence; _x15 is an obstacle point, and its class sequence is calculated to be 2+1=3; _x16 is an obstacle point, and its left point _x15 is detected to be an obstacle point, so the class sequence of _x15 is used, and the class sequence is 3.

3) For the nodes on the i-th row (i=2,…,m), perform the following operations:

a) Visit node x _i1 . If it is an obstacle node, check the node above it, x _(i-1)1 , and the node to the upper right, x _(i-1)2 : If both x _(i-1)1 and x _(i-1)2 have not been classified, the class order of x _i1 is the current total number of classes plus 1; if x _(i-1)1 has been classified and x _(i-1)2 has not been classified, then x _i1 follows the class order of x _(i-1)1 ; if x _(i-1)2 has been classified and x _(i-1)1 has not been classified, then x _i1 follows the class order of x _(i-1)2 ; if both x _(i-1)1 and x _(i-1)2 have been classified and have the same class order, then x _i1 follows the class order of x _(i-1)1 and x _(i-1)2 ; if both x _(i-1)1 and x _(i-1)2 have been classified and have different class orders, then x _i1 follows the class order of x (i-1)1 and replaces x _{(i-1)2 with x (i-1)1} . _(i-1)1 and x _(i-1)2 are recorded as an equivalent pair;

b) Visit node _xij (j=2,...,n-1). If it is an obstacle node, then detect its left node _xi(j-1) , upper left node x _(i-1)(j-1) , upper node x _(i-1)j and upper right node x _{(i-1)(j+1) in turn} : if these four points are not obstacle nodes, then the class order of _xij is the current total number of classes plus 1; if there is only one obstacle node among the four points, then _xij uses the class order of the obstacle point; if there are 2 to 4 obstacle nodes among the four points, then _xij uses the class order of the first point among the four points detected as an obstacle node, and the point pairs with different class orders among the four points are recorded as an equivalent pair.

c) Visit node x _in . If it is an obstacle node, then check its left node x _i(j-1) , upper left node x _(i-1)(j-1) and upper node x _{(i-1)j in turn} : if none of the three points are obstacle nodes, then the class order of x _ij is the current total number of classes plus 1; if there is only one obstacle node among the three points, then x _ij uses the class order of the obstacle node; if there are 2 to 3 obstacle nodes among the three points, then x _ij uses the class order of the first point detected as an obstacle node among the three points, and the point pairs with different class orders are recorded as an equivalent pair;

d) According to the equivalent pairs obtained above, the class order of equivalent categories is unified; all the obstacle points with the same class order above form an obstacle area on the binary map.

Then, the identified obstacle area is converted into a polygon, as shown in Figure 4. The specific method is as follows:

Find the maximum and minimum values of the horizontal and vertical coordinates of all points in the obstacle area: X _max , X _max , Y _max and Y _min , and construct a rectangular area covering the entire obstacle area with (X _min , Y _min ), (X _min , Y _max ), (X _max , Y _min ), (X _max , Y _max ) as vertices. Then, with a certain accuracy acc (abbreviation for accurate), divide the rectangular area into a×b rectangular sub-areas of the same shape, where a≈(Y _max -Y _min )/acc, b≈(X _max -X _max )/acc. Traverse all sub-areas. If there is no obstacle node on the rectangular sub-area, discard the sub-area. Finally, connect the outer vertices in the retained sub-areas to obtain the approximate polygon of the original obstacle.

Step 3: Decompose the free space on the mission flight surface into multiple convex polygons

After the obstacles in the mission flight surface are converted into polygons, the free space on the mission flight surface can be regarded as a multi-connected concave polygon. To use the A* algorithm based on graphics preprocessing, the free space in the mission flight surface needs to be decomposed into multiple convex polygons. This process first requires converting the multi-connected concave polygon into a single-connected concave polygon. The specific principle process is as follows:

As shown in Figure 5, when there is an obstacle area in the concave polygonal area surrounded by points _M1M2 … _Mn (the obstacle area _is surrounded by obstacle vertices _{O1O2 … On} ), select a vertex _Oi of the obstacle area and a vertex _Mi of the concave polygonal area to connect the line, denoted _{as OiMi} ; its vector can have two directions and/> Let two vectors/> With/> There is a distance ΔD→0 between the vertex _Oi of the obstacle and the vertex _Mi of the map through two vectors, and the map becomes a simply connected domain map M ₁ M ₂ …M _i O _i O _i+1 …O _n O ₁ …O _i M _i …M _n . If there are other obstacles in the concave polygon area, each obstacle needs to be directly or indirectly connected to the vertex of the map boundary.

If there is another obstacle O′ ₁ O′ ₂ …O′ _n , the direct connection is to connect a vertex O′ _i of the obstacle with a vertex M _j on the map boundary in the same way as above. The subsequent simply connected region is expressed as:

M ₁ M ₂ …M _i O _i O _i+1 …O _n O ₁ …O _i M _i …M _j O′ _i O′ _i+1 …O′ _n O′ ₁ …O′ _i M _j …M _n

Indirect connection means that obstacles can be connected to concave polygonal areas by connecting to transformed obstacles. If there is an obstacle connected to the concave polygonal area before, the boundary of this obstacle can be regarded as part of the boundary of the concave polygonal area. Then the new obstacle can be connected to the concave polygonal area by connecting the vertices of the transformed obstacle. Let the O′ _i point of the new obstacle be connected to the O _m point of the first obstacle, then the simply connected area after the connection is expressed as:

M ₁ M ₂ …M _i O _i O _i+1 …O _m O′ _i O′ _i+1 …O′ _n O′ ₁ …O′ _i O _m …O _n O ₁ …O _i M _i …M _n

After converting the multiply connected concave polygon into a simply connected concave polygon, it is necessary to decompose the simply connected concave polygon into several sub-convex polygons, as shown in Figure 6. The specific process is as follows:

a. Determine the positivity of the simply connected concave polygon. If it is a negative polygon, the vertices in its vertex set {P _n } are reordered in reverse order. If it is a regular polygon, proceed to step b.

b. Select a concave point P _k from {P _n }, search for all visible points of this point, and form a visible point set {V _k };

c. If there are concave points in {V _k }, then select the best visible point of P _k among these concave points; if there are no concave points in {V _k }, then select the best visible point of P _k in {V _k };

d. Connect P _k to its optimal visible point, decompose the map, and add a new sub-polygon;

e. Repeat steps b) to d) until there are no concave points in {P _n }, and all sub-polygons of the polygon are convex polygons.

The positive and negative values and the optimal visible point of the above polygon are defined as follows:

Positive and negative polygons: If the vertex sequence in the vertex set {P _n } of the polygon is arranged in counterclockwise order, then the polygon is a positive polygon, otherwise, the polygon is a negative polygon. The specific judgment method is: take any convex point P _i on the polygon, if it and its two adjacent points P _i-1 and P _i+1 form a vector and/> Satisfies the relationship:/> Then the polygon is positive, otherwise, the polygon is negative.

Optimal visible point: As shown in Figure 7, take the two adjacent points Mi _-1 and Mi ₊₁ of the concave point _Mi , and introduce the vector and/> The angle bisector of the angle formed/> Take a visible point M _k from the visible point set {V _k } of _Mi ,/> With vector /> Constitute the angle α and calculate its cosine value/> A visible point in {V _k } that maximizes this value is called the optimal visible point of _Mi.

Step 4: Use the A* algorithm based on graph preprocessing to find the optimal trajectory

First, it is necessary to process each sub-free space on the mission flight surface (the mission flight surface is processed using the previous method of processing polygons, and the sub-polygons that are not obstacles in the result are called sub-free spaces) and process them into nodes used by the A* algorithm. The specific method is as follows:

Add node numbers, node coordinates, adjacent node numbers, parent node numbers, and heuristic information to the child free space. For a child free space, the meaning of the above information is as follows:

The node number is the number of the sub-free space among all sub-free spaces;

The node coordinates are the centroid coordinates of the corresponding convex polygon (with vertices P ₁ , P ₂ , ..., P _n ) of the sub-free space;

The adjacent node number indicates the corresponding node number of other sub-areas adjacent to the area indicated by the node;

The parent node serial number is the serial number of the node in the sub-free space adjacent to the sub-free space that has the lowest cost to reach the sub-free space;

The heuristic information is the Euclidean distance between the centroids of two sub-free spaces, also known as the track cost.

Then, the A* algorithm is used to search for the optimal regional channel as follows:

(1) Add the node S _start in the region where the starting point X _start is located to the Open List of the sequence to be searched;

(2) If the Open List is empty, there is no feasible track and the algorithm ends; otherwise, proceed to step (3).

(3) Traverse the Open List and find the node S _curent with the smallest F value (F value represents the estimated path cost from the starting point through the current point S _curent to the end point), move it into the Close List, and remove it from the Open List;

(4) For each neighboring node S _neighbour of the node S _curent that is not in the Close List: if the node S _neighbour is not in the Open List, move it to the Open List, set the node S _curent as the parent node of the node S _neighbour , and update its G value (the G value is the actual track cost from the starting point to the current point) and F value; if the node S _neighbour is in the Open List, determine whether the G value of the node S _neighbour obtained via the node S _curent to the node S _neighbour is less than its original G value; if so, set the node S _curent as the parent node of the node S _neighbour , and update its G value and F value;

(5) Repeat steps (2) to (4) until the node corresponding to the area where the end point is located is in the Open List;

(6) Starting from the node corresponding to the area where the end point is located, each node _Si (no relationship. Here, each node refers to a node in the process of generating the track, i.e., the end point, the parent node of the end point, the parent node of the parent node of the end point, ..., until the starting point) is connected in sequence with its parent node _{Si_parent} to form a regional channel { _Si }.

After obtaining the regional channel {S _i }, the initial feasible track connecting the starting point and the end point can be generated in {S _i }. The track obtained at this time is not optimal yet and needs to be further optimized. The specific optimization method is:

① Let the number of global searches i = 0;

② Let the number of local searches j = 0;

③ Take the midpoints {M ₁ ,M ₂ ,…M _n } on the common dividing line of each pair of adjacent areas in the regional channel, and connect the initial feasible track {X _current }:X _start →M ₁ →M ₂ ...→M _n →X _end through these midpoints.

④ Randomly select a segmentation line Q _k Q _k+1 , adjust the track node M _k →R _k (R _k is another point on the segmentation line), and reconnect it to get a new track

{X _new }:X _start →M ₁ →M ₂ ... →R _k → ... →M _n →X _end

And calculate its length.

⑤ If the new track {X _new } is shorter than the current track {X _current }, replace the current track with the new track: {X _current }←{X _new }, let j=0; if the new track is longer than the current track, let j=j+1;

⑥ Repeat steps ④ and ⑤ until j is greater than the local search upper limit t, and let i = i + 1;

⑦ Repeat steps ③ to ⑥ until i is greater than the global search upper limit T.

The above local search upper limit t and global search upper limit T represent the maximum values of local optimization times and global optimization times respectively.

The track obtained at this time is shown in Figure 8. This track is displayed on the original mission flight surface as shown in Figure 9. Adding the height dimension information to the obtained track, the final three-dimensional track is shown in Figure 10.

Claims (5)

1. A three-dimensional track planning method based on free space and A-algorithm is characterized in that: the specific steps are as follows:

Step 1: extracting all nodes with corresponding heights from the terrain elevation model according to the given planning space height dimension to form a mission flight curved surface;

Step 2: identifying and distinguishing each obstacle region in the mission flight curved surface by using a binary image connected domain detection algorithm, and further converting the obstacle on the mission flight curved surface into a polygon;

Step 3: the free space on the mission flight curved surface can be regarded as a multi-communicated concave polygon, and the multi-communicated concave polygon is firstly converted into a single-communicated concave polygon; then decomposing the single communicated concave polygon into a plurality of sub-convex polygons;

step 4: using an A-x algorithm based on graphic preprocessing to find an optimal track;

Firstly, processing each sub free space on a task flight curved surface into a node used by an A-type algorithm; the specific method comprises the following steps:

Adding node serial numbers, node coordinates, adjacent node serial numbers, father node serial numbers and heuristic information into the child free space;

then searching an optimal area channel by using an A-algorithm;

after the optimal area channel is obtained, generating an initial feasible track for connecting the starting point and the end point in the optimal area channel; further optimizing the obtained flight path; the optimization method comprises the following steps:

① Let global search number i=0;

② Let local search number j=0;

③ Taking the midpoint { M ₁,M₂,…M_n } on the common dividing line of each pair of adjacent areas in the area channel, and connecting an initial feasible track { X _current}:X_start→M₁→M₂...→M_n→X_end } through the midpoint { M ₁,M₂,…M_n };

④ Randomly selecting a dividing line Q _kQ_k+1, adjusting the track node M _k→R_k,R_k thereon to be another point on the dividing line, and reconnecting to obtain a new track

{X_new}:X_start→M₁→M₂...→R_k→...→M_n→X_end

And calculating the length thereof;

⑤ If the new track { X _new } is shorter than the current track { X _current }, then the current track is replaced with the new track: { X _current}←{X_new }, let j=0; if the new track is longer than the current track, j=j+1;

⑥ Repeating steps ④ and ⑤ until j is greater than the local search upper limit t, letting i=i+1;

⑦ Step ③～⑥ is repeated until i is greater than the global search upper limit T.

2. A three-dimensional track planning method based on free space and a-algorithm as claimed in claim 1, wherein: in the step 2, the binary map matrix is divided into m rows and n columns, and the specific method for identifying and distinguishing the obstacle area comprises the following steps:

1) If the access node x ₁₁ is an obstacle node, the class sequence of the access node x ₁₁ is recorded as 1;

2) Traversing node x _1j (j=2,., n), if it is an obstacle node, detecting if its left node x _1(j-1) is an obstacle node; if so, x _1j follows the class order of x _1(j-1); otherwise, the class sequence of x _1j is the current total class number (the total number of the class sequences of the obstacle nodes) plus 1;

3) For the node (i=2, …, m) on the i-th row, the following operation is performed:

a) If the access node x _i1 is an obstacle node, the access node x _(i-1)1 and the upper right node x _(i-1)2 are detected: if neither x _(i-1)1 nor x _(i-1)2 are classified, the class order of x _i1 is the current total class number plus 1; if x _(i-1)1 has been classified and x _(i-1)2 has not, then x _i1 follows the class of x _(i-1)1; if x _(i-1)2 has been classified and x _(i-1)1 has not, then x _i1 follows the class of x _(i-1)2; if x _(i-1)1 and x _(i-1)2 are both classified and the class order is the same, then x _i1 follows the class order of x _(i-1)1 and x _(i-1)2; if x _(i-1)1 and x _(i-1)2 are both classified and the class order is different, then x _i1 follows the class order of x _(i-1)1 and marks x _(i-1)1 and x _(i-1)2 as an equivalent pair;

b) Access node x _ij (j=2,., n-1), if it is an obstacle node, detects its left node x _i(j-1), upper left node x _(i-1)(j-1), upper right node x _(i-1)j, and upper right node x _(i-1)(j+1) in order: if the four points are all non-obstacle nodes, the class sequence of x _ij is the current total class number plus 1; if the four points have only one obstacle node, x _ij follows the class sequence of the obstacle node; if 2-4 barrier nodes exist in the four points, x _ij takes the class sequence of the first point detected as the barrier node in the four points, and marks the point pairs with different class sequences in the four points as an equivalent pair;

c) If the access node x _in is an obstacle node, the access node x _i(j-1), the upper left node x _(i-1)(j-1), and the upper node x _(i-1)j are sequentially detected: if all the three points are non-obstacle nodes, the class sequence of x _ij is the current total class number plus 1; if the three points have only one obstacle node, x _ij follows the class sequence of the obstacle node; if 2-3 barrier nodes exist in the three points, x _ij takes the class sequence of the first detected point as the barrier node in the three points, and marks the point pairs with different class sequences as an equivalent pair;

d) Unifying class sequences of the equivalent classes according to the obtained equivalent pairs; all the obstacle points with the same class sequence form an obstacle area on the binary map;

Subsequently, the identified obstacle region is converted into a polygon by the following specific method:

Finding the maximum and minimum values in the abscissa of all points on the obstacle region: x _max,X_max,Y_max and Y _min, with (X _min,Y_min),(X_min,Y_max),(X_max,Y_min),(X_max,Y_max) as the vertex, construct a rectangular area covering the whole obstacle area; then, dividing the rectangular region into a multiplied by b rectangular subregions with the same shape with a certain precision acc, wherein a is approximately equal to (Y _max-Y_min)/acc,b≈(X_max-X_max)/acc; traversing all the subareas, and discarding the subareas if no barrier node exists on the rectangular subareas;

And finally, connecting the outer vertexes in the reserved subareas to obtain the approximate polygon of the original obstacle.

3. A three-dimensional track planning method based on free space and a-algorithm as claimed in claim 1, wherein: in the step 3, the specific method for converting the multi-communicated concave polygon into the single-communicated concave polygon comprises the following steps:

when an obstacle area exists in the concave polygon area surrounded by the points M ₁M₂…M_n, selecting a vertex O _i of the obstacle area to be connected with one vertex M _i of the concave polygon area, and marking as O _iM_i; the vector of which has two directions And/>Let two vectors/>And/>The distance delta D-0 is arranged between the two points, and the vertex O _i of the obstacle is connected with the vertex M _i of the map through two vectors, so that the map becomes a single connected domain map M₁M₂…M_iO_iO_i+1…O_nO₁…O_iM_i…M_n;

If there are other obstacles in the concave polygon area, each obstacle needs to be directly or indirectly connected to the map boundary vertex.

4. A three-dimensional track planning method based on free space and a-algorithm as claimed in claim 1, wherein: in the step 3, the specific method for decomposing the single communicated concave polygon into a plurality of sub-convex polygons comprises the following steps:

a. judging the positive and negative of the single-communication concave polygon, if the single-communication concave polygon is a negative polygon, reordering the vertexes in the vertex set { P _n } of the single-communication concave polygon, and if the single-communication concave polygon is a positive polygon, entering the step b;

b. Selecting a concave point P _k from { P _n }, searching all visible points of the point to form a visible point set { V _k };

c. If there is a pit in { V _k }, then select the best visible point of P _k among the pits, and if there is no pit in { V _k }, then select the best visible point of P _k among { V _k };

d. Connecting P _k with the optimal visible point, decomposing the map, and adding a new sub-polygon;

e. Repeating steps b) to d) until no pits are present in { P _n }, and then all sub-polygons of the polygon are convex polygons.

5. A three-dimensional track planning method based on free space and a-algorithm as claimed in claim 1, wherein: in step 4, the method for searching the optimal area channel by using the algorithm a is as follows:

(1) Adding a node S _start of the region where the starting point X _start is located into an Open List of the sequence to be retrieved;

(2) If the OpenList is empty, no feasible track exists, and the algorithm is ended; otherwise, performing the step (3);

(3) Traversing the OpenList, searching a node S _curent with the minimum F value, moving the node S _curent into the Close List, and moving the node S out of the OpenList; the F value is the estimated track cost from the starting point to the end point through the current point S _curent;

(4) For each neighbor node S _neighbour of nodes S _curent that is not in the Close List: if node S _neighbour is not in the Open List, it is moved into the Open List, and node S _curent is set as the parent node of node S _neighbour, updating its G and F values; the G value is the actual track cost from the starting point to the current point; if the node S _neighbour is in the Open List, determining whether the G value of the node S _neighbour obtained from the node S _curent to the node S _neighbour is smaller than the original G value; if yes, setting the node S _curent as a father node of the node S _neighbour, and updating the G value and the F value of the father node;

(5) Repeating the steps (2) - (4) until the corresponding node of the area where the terminal point is located is in the Open List;

(6) Each node S _i is connected in turn to its parent node S _{i_parent} from the node corresponding to the region where the endpoint is located, forming an optimal region channel { S _i }.