CN105737819A - Unmanned aerial vehicle three-dimensional airway planning method based on space compression and table-lookup calculation - Google Patents

Abstract

The invention provides an unmanned aerial vehicle three-dimensional airway planning method based on space compression and table-lookup calculation, and mainly relates to the field of airway planning, in particular to the field of unmanned aerial vehicle three-dimensional airway planning. Unmanned aerial vehicle online rapid three-dimensional airway planning is achieved. On the basis of the standard A* algorithm, jump extension of the similar sparse A * algorithm is adopted, child nodes are changed to plane discretization distribution, and by means of a way of optimal compression in the height direction, the relative relation between the child nodes and a father node is calculated in advance, tabulated and stored; the table is looked up to obtain a value for each time of extension, the search space data structure is improved, the planning space can be repeatedly used during repeated airway planning, and the reinitialization time is saved. According to the method, on the basis of reserving the advantages of an original algorithm, the number of the nodes is greatly reduced, the algorithm efficiency is improved, and the replanning ability of the algorithm is improved.

Description

No-manned plane three-dimensional Route planner based on space compression and computation of table lookup

Technical field

The present invention relates to routeing field, especially the three-dimensional routeing field of unmanned plane.

Background technology

The feature of risk that unmanned plane has that mobility is good, survival ability is strong, cost is low, efficiency-cost ratio is high and no one was injured, it is able to carry out aerial reconnaissance, over the ground strike, the over the ground multiple-task such as supervision and eavesdropping, electronic interferences, is defendance territorial waters, territorial sky, safeguards Homeland Security, strengthens the important component part of defense force.

Path Planning for Unmanned Aircraft Vehicle refers to one air route currently putting impact point from unmanned plane of searching in space, and this air route must is fulfilled for air route constraints and safety condition, and makes voyage short as far as possible, and it is the key realizing unmanned plane independent navigation and task guarantee.Path Planning for Unmanned Aircraft Vehicle problem is typical np problem, currently a lot of intelligent algorithm has been had to be applied in Path Planning for Unmanned Aircraft Vehicle, such as A* algorithm, Visual Graph method, Voronoi method, genetic algorithm, ant group algorithm, particle cluster algorithm and Artificial Potential Field Method etc., there is also problems, for instance computation complexity is big, pretreatment length consuming time, be easily absorbed in local optimum or there is search trap etc. while there is respective advantage.Particularly recently as the fast development of manufacturing technology, unmanned plane performance constantly strengthens, and the radius of clean-up constantly increases, and the complexity of routeing problem is explosive growth along with problem scale.Being limited to computing capability and the memory space of airborne computer, three-dimensional Route planner conventional at present is difficult to meet the demand of the online routeing of unmanned plane.The present invention is according to the online routeing feature of unmanned plane, propose a kind of Route Planning Algorithm based on space compression and computation of table lookup, three dimensions routeing problem can be solved by two-dimensional map space, substantially reduce the required operation time of planning and memory space, and there is good weight-normality draw ability.

Summary of the invention

In order to overcome the deficiencies in the prior art, The present invention gives a kind of online quick three-dimensional Route planner of unmanned plane, the method is on the basis of standard A* algorithm, the jump adopting similar sparse A* algorithm extends, but child node changes the distribution of plane discretization into, and by the mode of preferred compressed in the height direction, by the relativeness calculated in advance between child node and father node storage of tabulating, table look-up during extension value every time, without recalculating, simultaneously improve search volume data structure, repeatedly during routeing, planning space can be reused, save the time of reinitializing.

The detailed step of the technical solution adopted for the present invention to solve the technical problems is as follows:

Step one: initialize and environmental modeling

The horizontal plane of unmanned plane range of activity is projected even partition is Maxrow × Maxcol grid, grid size keeps consistent with the terrain data file precision adopted, grid line number Maxrow and grid columns Maxcol is obtained divided by grid size by unmanned plane range of activity, build and initialize the planning space matrix M mapping as actual three dimensional search space of a Maxrow × Maxcol, the wherein maximum number of lines of Maxrow and Maxcol respectively planning space matrix M and maximum number of column, build the Open list as A* algorithm of the empty complete minimum Binary Heap, the global variable initializing planning space matrix M reuses identifier；

Wherein, the each grid point of planning space matrix M represents a search node of heuristic search, its abscissa and vertical coordinate representation node horizontal level, each nodal stored information includes actual cost value g, residue cost estimated value h, total cost estimated value f, reuse mark pointmode, unmanned plane current course angle Φ, pitching angle theta, parent information, optimal height besth and grid point Terrain Elevation dem, wherein except Terrain Elevation dem, all the other parameters are all initialized as 0, dem takes this some place Terrain Elevation, if but there are obstacle or threat in certain node place, then the dem of this node takes obstacle or threatens the coboundary height in this node place coverage；In Open list, each element is each through a search node in pointer correspondence planning space matrix M, and it is initialized as sky table, global variable matrixflag is set and reuses identifier as planning matrix, and to initialize it be 0, if certain node is accessed in the future, then the reusing identifier pointmode and will be assigned matrixflag+1 of this node, if certain node is expanded, then the reusing identifier pointmode and will be assigned matrixflag+2 of this node；

Step 2: planning tasks parameter updates

Regeneration planning matrix reuses identifier matrixflag, according to task parameters, calculates Regeneration planning parameter storage of tabulating；

Detailed step is as follows:

The present invention gives for each node in planning space matrix M and reuses identifier value pointmode, same class node to reuse identifier pointmode identical, by checking that the identifier of reusing of node confirms node state, each planning tasks reuse identifier value difference, arrange each close point of expanding node in this planning and be labeled as pointmode=matrixflag+2, and remember that the value of matrixflag+2 is pointclose；Each access but non-expanding node or need the open point of expanding node to be again labeled as pointmode=matrixflag+1, and remember that the value of matrixflag+1 is pointopen, the node of the so all pointmode of meeting≤matrixflag is automatically recognized as this and plans the new node new node not yet accessed；

Map that in planning space matrix M according to starting point, impact point horizontal level, and substitute corresponding point optimal height besth with its starting point, impact point height；

Calculating this projecting parameter, when the way point precision of mission requirements is Np, then single leg length Δ L is:

$\mathrm{\Δ}L=\sqrt{2}\×dis(start,goal)/Np$

Wherein start is starting point, and goal is terminal, and dis (start, goal) is air line distance between starting point and impact point；

When mission requirements directional precision is Na, the unmanned plane maximum turning angle in length Δ L leg is φ max, then calculate and store horizontal-shift discrete sheet:

$\left\{\begin{array}{c}da=floor(0.5\×Na\×\mathrm{\φ}max/\mathrm{\π})\\ \mathrm{\Δ}row\left(i\right)=floor\[\mathrm{\Δ}L\×\cos (i\×2\mathrm{\π}/Na)/rowheight\]\\ \mathrm{\Δ}col\left(i\right)=floor\[\mathrm{\Δ}L\×\sin (i\×2\mathrm{\π}/Na)/colwide\]\end{array}\right.$

Wherein da is single extension maximum half span in the horizontal direction, floor is downward bracket function, Δ row (i) extends the child node on the i-th direction relative to father node relative displacement in line number for single, Δ col (i) extends child node on the i-th direction relative to father node relative displacement on columns for single, i=0, and 1,2,3 ... Na, rowheight are planning space line space, colwide is planning space column pitch；

When mission requirements height-precision is Nh, maximum climb/gliding angle is θ max, unmanned plane ceiling height is maxh, then calculate and storing highly skew discrete sheet:

$\left\{\begin{array}{c}\mathrm{\Δ}h=floor(\max h/Nh)\\ dh=floor\[\mathrm{\Δ}L\×tan\left(\mathrm{\θ}\max \right)/\mathrm{\Δ}h\]\\ dish\left(k\right)=\sqrt{{\mathrm{\ΔL}}^2+{(k\×\mathrm{\Δ}h)}^2}\end{array}\right.$

Wherein Δ h is planning vertical separation, and dh is that single extends maximum span in the height direction, and diah (k) extends the child node at kth discrete height place on specific direction and the Euclidean distance approximation of father node for single, k=0,1,2,3 ... Nh；

Step 3: route searching

Iteration performs A* searching algorithm, until finding result path and exporting, detailed step is:

First starting point it is labeled as open node and puts into Open list, total cost estimated value f takes air line distance between starting point and impact point, during search, from Open list, take out the node of total cost estimated value f Least-cost as father node every time, if this node is impact point, then search terminates, and recalls from impact point along father node iteration, exports result path；Otherwise extend child node, father node is changed into close state；

nullDuring extension child node，Limited by the current course of unmanned plane and cornering ability，Present node has maximum 2 × da+1 child node，If impact point distance father node distance is less than single leg length Δ L，Then impact point is considered as an extra alternative child node，The height bound of each child node is subject to Terrain Elevation、Minimum liftoff safe altitude、Unmanned plane ceiling and father node unmanned plane height、The angle of pitch and single maximum height span dh restriction，If there are obstacle or threat in this child node place，Then wherein Terrain Elevation is the height after superposition threat or obstacle，This scope is carried out discretization with vertical separation Δ h，Each horizontal extension direction chooses to have the minimum height node of total cost estimated value be optimum alternative child node，If its total cost estimated value is less than total cost estimated value of the original node of this horizontal position in planning space matrix M，Then replace original node with this optimum alternate node，All be updated after node all reset to open state，Add Open list and resequence，Ensure that list internal node arranges from small to large according to total cost estimated value f all the time；

Step 4: amending plans space reuse identifies

After completing the task of step 2 and step 3, planning space is reused identifier matrixflag and is added 3, abandons all data of Open table, if any new planning tasks, jumps to step 2, waits that new task restarts.

The invention has the beneficial effects as follows due to compared with existing A* algorithm and sparse A* algorithm, the present invention is by by the child node distribution plane discretization in three-dimensional search, and preferred compressed in the height direction, three dimensional search space is mapped to 2-D data space, retaining on the basis of former algorithm advantage, substantially reduce interstitial content, reduce computational complexity, operation time and memory space consumption；By storage that the relative information between child node and father node is tabulated in advance so that every time during extension can table look at value, eliminate the time that repeatedly calculates, improve efficiency of algorithm；Reuse identifier by Global motion planning space reuse identifier and individual node to cooperate, eliminate Close table in A* algorithm, eliminate the query manipulation of Open table, and automatically identify the untreated node in planning every time, eliminate its interference to search procedure, eliminating the time initializing planning space when every time again planning, the weight-normality that improve algorithm draws ability.

Accompanying drawing explanation

Fig. 1 is the method flow diagram of the present invention.

Detailed description of the invention

Below in conjunction with drawings and Examples, the present invention is further described, but its parameter is not limitation of the invention.

Step one: initialize and environmental modeling

The horizontal plane of unmanned plane range of activity is projected even partition is Maxrow × Maxcol grid, grid size keeps consistent with the terrain data file precision adopted, grid line number Maxrow and grid columns Maxcol is obtained divided by grid size by unmanned plane range of activity, build and initialize the planning space matrix M mapping as actual three dimensional search space of a Maxrow × Maxcol, the wherein maximum number of lines of Maxrow and Maxcol respectively planning space matrix M and maximum number of column, build the Open list as A* algorithm of the empty complete minimum Binary Heap, the global variable initializing planning space matrix M reuses identifier；

Wherein, the each grid point of planning space matrix M represents a search node of heuristic search, its abscissa and vertical coordinate representation node horizontal level, each nodal stored information includes actual cost value g, residue cost estimated value h, total cost estimated value f, reuses identifier pointmode, unmanned plane current course angle Φ, pitching angle theta, parent information, optimal height besth and grid point Terrain Elevation dem, and wherein except Terrain Elevation dem, all the other parameters are all initialized as 0.Dem takes this some place Terrain Elevation, if but there are obstacle or threat in certain node place, then and the dem of this node takes obstacle or threatens the coboundary height in this node place coverage；In Open list, each element is each through a search node in pointer correspondence planning space matrix M, and it is initialized as sky table, global variable is reused identifier matrixflag and is initialized as 0, if certain node is accessed in the future, then the reusing identifier pointmode and will be assigned matrixflag+1 of this node, if certain node is expanded in the future, then the reusing identifier pointmode and will be assigned matrixflag+2 of this node；

Concretely comprise the following steps:

1.1 by unmanned plane range of activity rasterizing, builds two dimension planning space matrix M, covers whole zone of action.Such as, unmanned plane radius of action 100km, user requires that planning precision is not less than 100m, then, after rasterizing, M is the planning matrix of 1000 × 1000.Each grid point represents a search node of heuristic search, and M (row, col) .dem is corresponding planning matrix row row, the Terrain Elevation at col row place.If having threat object or no-fly zone, then using the height in threat/no-fly region somewhere as this region equivalent Terrain Elevation.In this example, grid line spacing and column pitch equal (nonessential), be all designated as 1 after ignoring dimension.

The 1.2 current cost value g initializing each search node, residue cost estimated value h and total cost estimated value f is 0；The identifier pointmode that reuses initializing each search node is 0；Initializing current planning space, to reuse identifier matrixflag be 0；The optimal height besth initializing each search node is 0, initializes each node unmanned plane course angle Φ and pitching angle theta is numbered 0；

1.3 initialize an empty orderly Binary Heap as Open list, certain search node in all corresponding planning matrix of each element in list, and arrange from small to large according to total cost estimated value.

Above initialization operation only performs when first time routeing, repeatedly during routeing, as long as unmanned plane zone of action does not change or without departing from original matrix M coverage, then planning matrix can directly be reused, it is not necessary to reinitializes.

Step 2: planning tasks parameter updates

According to the space reuse identifier in step one, update matrix and reuse identifier matrixflag, according to task parameters, calculate Regeneration planning parameter storage of tabulating；

Detailed step is as follows:

The present invention gives for each node in planning space matrix M and reuses identifier value pointmode, same class node to reuse identifier value pointmode identical, by checking that the identifier value of reusing of node confirms node state, each planning tasks reuse identifier value difference, arrange each close point of expanding node in this planning and be labeled as pointmode=matrixflag+2, and remember that the value of matrixflag+2 is pointclose；Each access but non-expanding node or need the open point of expanding node to be again labeled as pointmode=matrixflag+1, and remember that the value of matrixflag+1 is pointopen, the node of the so all pointmode of meeting≤matrixflag is automatically recognized as this and plans the new node new node not yet accessed；

Map that in planning space matrix M according to starting point, impact point horizontal level, and substitute corresponding point optimal height besth with its starting point, impact point height；

Calculating this projecting parameter, when the way point precision of mission requirements is Np, then single leg length Δ L is:

$\mathrm{\Δ}L=\sqrt{2}\×dis(start,goal)/Np$

Wherein start is starting point, and goal is terminal, and dis (start, goal) is air line distance between starting point and impact point；

When mission requirements directional precision is Na, the unmanned plane maximum turning angle in length Δ L leg is φ max, then calculate and store horizontal-shift discrete sheet:

$\left\{\begin{array}{c}da=floor(0.5\×Na\×\mathrm{\φ}max/\mathrm{\π})\\ \mathrm{\Δ}row\left(i\right)=floor\[\mathrm{\Δ}L\×\cos (i\×2\mathrm{\π}/Na)/rowheight\]\\ \mathrm{\Δ}col\left(i\right)=floor\[\mathrm{\Δ}L\×\sin (i\×2\mathrm{\π}/Na)/colwide\]\end{array}\right.$

Wherein da is single extension maximum half span in the horizontal direction, floor is downward bracket function, Δ row (i) extends the child node on the i-th direction relative to father node relative displacement in line number for single, Δ col (i) extends child node on the i-th direction relative to father node relative displacement on columns for single, i=0, and 1,2,3 ... Na, rowheight are planning space line space, colwide is column pitch；

When mission requirements height-precision is Nh, maximum climb/gliding angle is θ max, unmanned plane ceiling height is maxh, then calculate and storing highly skew discrete sheet:

$\left\{\begin{array}{c}\mathrm{\Δ}h=floor(\max h/Nh)\\ dh=floor\[\mathrm{\Δ}L\×tan\left(\mathrm{\θ}\max \right)/\mathrm{\Δ}h\]\\ dish\left(k\right)=\sqrt{{\mathrm{\ΔL}}^2+{(k\×\mathrm{\Δ}h)}^2}\end{array}\right.$

Wherein Δ h is planning vertical separation, and dh is that single extends maximum span in the height direction, and diah (k) extends the child node at kth discrete height place on specific direction and the Euclidean distance approximation of father node for single, k=0,1,2,3 ... Nh.

The present embodiment specifically performs as follows:

2.1 matrix condition information.Update the node of this planning to reuse identifier value and be, open state point pointopen=matrixflag+1, close state point pointclose=matrixflag+2.

2.2 starting points, impact point information.Starting point, impact point are respectively mapped to grid matrix, remember the behavior startrow at initial point place, place be classified as startcol, it is highly starth, note impact point place behavior goalrow, the row goalcol at place, it is highly goalh.

2.3 projecting parameter calculate

The way point precision assuming mission requirements is Np, and directional precision is Na, and height-precision is Nh, the unmanned plane maximum turning angle in length Δ L leg is φ max, maximum climb/gliding angle is θ max, unmanned plane ceiling height is maxh, then count then single leg length Δ L:

$\mathrm{\Δ}L=\sqrt{2}\×dis(start,goal)/Np$

Calculate and store horizontal-shift discrete sheet:

$\left\{\begin{array}{c}da=floor(0.5\×Na\×\mathrm{\φ}max/\mathrm{\π})\\ \mathrm{\Δ}row\left(i\right)=floor\[\mathrm{\Δ}L\×\cos (i\×2\mathrm{\π}/Na)/rowheight\]\\ \mathrm{\Δ}col\left(i\right)=floor\[\mathrm{\Δ}L\×\sin (i\×2\mathrm{\π}/Na)/colwide\]\end{array}\right.$

Calculate and storing highly skew discrete sheet:

$\left\{\begin{array}{c}\mathrm{\Δ}h=floor(\max h/Nh)\\ dh=floor\[\mathrm{\Δ}L\×tan\left(\mathrm{\θ}\max \right)/\mathrm{\Δ}h\]\\ dish\left(k\right)=\sqrt{{\mathrm{\ΔL}}^2+{(k\×\mathrm{\Δ}h)}^2}\end{array}\right.$

Wherein dish (k) extends the child node of kth place discrete height on specific direction and the Euclidean distance approximation of father node for single, and k=0,1,2,3 ... Nh.

In this example, take Np=20, Nh=80, φ max=π/4, θ max=π/4, maxh=160, calculate Δ L=10, Δ h=2, da=5, dh=5.

Step 3: route searching

Iteration performs A* searching algorithm, until finding result path and exporting, detailed step is:

First starting point it is labeled as open node and puts into Open list, total cost estimated value f takes air line distance between starting point and impact point, during search, from Open list, take out the node of total cost estimated value f Least-cost as father node every time, if this node is impact point, then search terminates, and recalls from impact point along father node iteration, exports result path；Otherwise extend child node, father node is changed into close state；

nullDuring extension child node，Limited by the current course of unmanned plane and cornering ability，Present node has maximum 2 × da+1 child node，If impact point distance father node distance is less than single leg length Δ L，Then impact point is considered as an extra alternative child node，The height bound of each child node is subject to Terrain Elevation、Minimum liftoff safe altitude、Unmanned plane ceiling and father node unmanned plane height、The angle of pitch and single maximum height span dh restriction，If there are obstacle or threat in this child node place，Then wherein Terrain Elevation is the height after superposition threat or obstacle，This scope is carried out discretization with vertical separation Δ h，Each horizontal extension direction chooses to have the minimum height node of total cost estimated value be optimum alternative child node，If its total cost estimated value is less than total cost estimated value of the original node of this horizontal position in planning space matrix M，Then replace original node with this optimum alternate node，All be updated after node all reset to open state，Add Open list and resequence，Ensure that list internal node arranges from small to large according to total cost estimated value f all the time.

Step is as follows in the present embodiment:

3.1 initialize the search node representing starting point in planning matrix, and its node state is set to pointmode=pointopen, and is put in Open list by this node.

3.2 judge whether Open table is empty, if being sky, and algorithm search failure；Otherwise continue.

If 3.3 Open tables are not empty, take out first element (search node that estimate cost is minimum) in Open list, be designated as pNode.If this node is impact point, jump to step 3.5；Otherwise jump to step 3.4.

3.4 extension present nodes

Present node has maximum 2 × da+1 child node (if unmanned plane original heading does not limit, then starting point has Na child node), initializes j=0, performs operation extended below:

3.4.1 child node horizontal level is determined

If had:

$\left\{\begin{array}{c}0<pNode.row+\mathrm{\&Delta;}row\&lsqb;\mathrm{mod}(j+pNode.\mathrm{\&phi;}-da+Na,Na)\&rsqb;\&le;Maxrow\\ 0<pNode.col+\mathrm{\&Delta;}col\&lsqb;\mathrm{mod}(j+pNode.\mathrm{\&phi;}-da+Na,Na)\&rsqb;\&le;Maxcol\end{array}\right.$

Wherein Maxrow, the maximum number of lines of Maxcol respectively planning space matrix and maximum number of column, pNode.row is node pNode line number at place in M, pNode.col is the columns at place in M, mod (x, y) representing the positive integer x result to positive integer y remainder, pNode. φ is the course angle of node pNode, then present node child node CNode (j) in jth horizontal direction has:

$\left\{\begin{array}{c}CNode\left(j\right).row=pNode.row+\mathrm{\&Delta;}row\&lsqb;\mathrm{mod}(j+pNode.\mathrm{\&phi;}-da+Na,Na)\&rsqb;\\ CNode\left(j\right).col=pNode.col+\mathrm{\&Delta;}col\&lsqb;\mathrm{mod}(j+pNode.\mathrm{\&phi;}-da+Na,Na)\&rsqb;\\ CNode\left(j\right).\mathrm{\&phi;}=\mathrm{mod}(j+pNode.\mathrm{\&phi;}-da+Na,Na)\end{array}\right.$

CNode (j) .row is node CNode (j) line number at place in M, CNode (j) .col is node CNode (j) columns at place, CNode (j) in M. φ is the course angle of node CNode (j).

3.4.2 optimal height is determined

Child node CNode (j) altitude range is:

$\left\{\begin{array}{c}h1=\min \left\{pNode.besth+dh\&times;\mathrm{\&Delta;}h,\max h\right\}\\ h2=\max \left\{pNode.besth-dh\&times;\mathrm{\&Delta;}h,CNode,dem+\min h\right\}\end{array}\right.$

Wherein, h1, h2 is child node CNode (j) upper height limit and lower limit, pNode.besth is the unmanned plane height at pNode point place, maxh is the unmanned plane upper limit, CNode.dem is the Terrain Elevation at this some place, and minh is the minimum liftoff safe altitude of unmanned plane, and taking minh in this example is 5 unit lengths.Assume CNode (j, k) for floor projection at CNode (j), is highly the spatial point of k × Δ h, wherein h1≤k × Δ h≤h2, k ∈ N, then optimum child node height is:

$\left\{\begin{array}{c}CNode\left(j\right).besth=k_{best}\&times;\mathrm{\&Delta;}h\\ CNode\left(j\right).\mathrm{\&theta;}=floor(k_{best}-pNode.\mathrm{\&theta;})\end{array}\right.$

Wherein k_bestTake and in scope, make dis [pNode, CNode (j, k)]+dis [CNode (j, k), goal] k value that minimizes, in same planning tasks, CNode (j, k) limited with CNode (j) relative position situation, its relative distance has calculated in step 2 and has stored, can table look at value:

Dis [pNode, CNode (j, k)]=dish (abs (k-floor (pNode.besth/ Δ h)))

Wherein abs is the function that takes absolute value.

Separately have:

$\left\{\begin{array}{c}CNode\left(j\right).g=pNode.g+dis\left(pNode,CNode\left(j,k_{best}\right)\right)\\ CNode\left(j\right).h=dis\&lsqb;CNode\left(j,k_{best}\right),goal\&rsqb;\\ CNode\left(j\right).f=CNode\left(j\right).g+CNode\left(j\right).h\end{array}\right.$

If the distance between father node pNode and impact point is less than minimum flight path segment length Δ L, impact point node is as the alternative child node of an extra interpolation, and processing method is identical with other nodes.

3.4.3 Regeneration planning space

(1) if node corresponding for CNode (j) in planning matrix is not yet accessed, it may be assumed that

M(CNode(j).row,CNode(j).col).pointmode<pointopen

Then CNode (j) substitutes this node of planning matrix Central Plains, and add in Open list, revise node state, make M (CNode (j) .row, CNode (j) .col) .pointmode=pointopen, then rearrangement Open list.

(2) if to be total cost estimated value of open node and CNode (j) before this extension less than origin node for node originally corresponding for CNode (j) in planning matrix, namely

$\left\{\begin{array}{c}M\left(CNode\right(j).row,CNode(j).col).po\mathrm{int}\mathrm{mod}e==po\mathrm{int}open\\ M\left(CNode\right(j).row,CNode(j).col).f>CNode\left(j\right).f\end{array}\right.$

Then CNode (j) substitutes in planning matrix and this node of Open list Central Plains, and is rearrangement Open list.

(3) if to be total cost estimated value of close node and CNode (j) before this extension less than origin node for node originally corresponding for CNode (j) in planning matrix, namely

$\left\{\begin{array}{c}M\left(CNode\right(j).row,CNode(j).col).po\mathrm{int}\mathrm{mod}e==po\mathrm{int}close\\ M\left(CNode\right(j).row,CNode(j).col).f>CNode\left(j\right).f\end{array}\right.$

Then CNode (j) substitutes this node of planning matrix Central Plains, and add in Open list, revise node state, make M (CNode (j) .row, CNode (j) .col) .pointmode=pointopen, then rearrangement Open list.

If the condition of above (1), (2), (3) is all unsatisfactory for, then illustrate that new node does not improve air route quality, it is not necessary to Regeneration planning matrix.

If 3.4.4 j < 2 × da, then j=j+1, return 3.4.1；Otherwise jump to 3.2.

3.5 date back starting point from impact point according to its father node iteration, export result path

Step 4: amending plans space reuse identifier

Amending plans space reuse identifies, make matrixflag=matrixflag+3, in order to planning next time, after completing the task of step 2 and step 3, planning space is reused identifier matrixflag and is added 3, abandons all data of Open table, if any new planning tasks, jump to step 2, wait that new task restarts.

Claims (1)

1. the no-manned plane three-dimensional Route planner based on space compression and computation of table lookup, it is characterised in that comprise the steps:

Step one: initialize and environmental modeling

The horizontal plane of unmanned plane range of activity is projected even partition is Maxrow × Maxcol grid, grid size keeps consistent with the terrain data file precision adopted, grid line number Maxrow and grid columns Maxcol is obtained divided by grid size by unmanned plane range of activity, build and initialize the planning space matrix M mapping as actual three dimensional search space of a Maxrow × Maxcol, the wherein maximum number of lines of Maxrow and Maxcol respectively planning space matrix M and maximum number of column, build the Open list as A* algorithm of the empty complete minimum Binary Heap, the global variable initializing planning space matrix M reuses identifier；

Wherein, the each grid point of planning space matrix M represents a search node of heuristic search, its abscissa and vertical coordinate representation node horizontal level, each nodal stored information includes actual cost value g, residue cost estimated value h, total cost estimated value f, reuse mark pointmode, unmanned plane current course angle Φ, pitching angle theta, parent information, optimal height besth and grid point Terrain Elevation dem, wherein except Terrain Elevation dem, all the other parameters are all initialized as 0, dem takes this some place Terrain Elevation, if but there are obstacle or threat in certain node place, then the dem of this node takes obstacle or threatens the coboundary height in this node place coverage；In Open list, each element is each through a search node in pointer correspondence planning space matrix M, and it is initialized as sky table, global variable matrixflag is set and reuses identifier as planning matrix, and to initialize it be 0, if certain node is accessed in the future, then the reusing identifier pointmode and will be assigned matrixflag+1 of this node, if certain node is expanded, then the reusing identifier pointmode and will be assigned matrixflag+2 of this node；

Step 2: planning tasks parameter updates

Regeneration planning matrix reuses identifier matrixflag, according to task parameters, calculates Regeneration planning parameter storage of tabulating；

Detailed step is as follows:

The present invention gives for each node in planning space matrix M and reuses identifier value pointmode, same class node to reuse identifier pointmode identical, by checking that the identifier of reusing of node confirms node state, each planning tasks reuse identifier value difference, arrange each close point of expanding node in this planning and be labeled as pointmode=matrixflag+2, and remember that the value of matrixflag+2 is pointclose；Each access but non-expanding node or need the open point of expanding node to be again labeled as pointmode=matrixflag+1, and remember that the value of matrixflag+1 is pointopen, the node of the so all pointmode of meeting≤matrixflag is automatically recognized as this and plans the new node new node not yet accessed；

Map that in planning space matrix M according to starting point, impact point horizontal level, and substitute corresponding point optimal height besth with its starting point, impact point height；

Calculating this projecting parameter, when the way point precision of mission requirements is Np, then single leg length Δ L is:

$\mathrm{\&Delta;}L=\sqrt{2}\&times;dis(start,goal)/Np$

Wherein start is starting point, and goal is terminal, and dis (start, goal) is air line distance between starting point and impact point；

When mission requirements directional precision is Na, the unmanned plane maximum turning angle in length Δ L leg is φ max, then calculate and store horizontal-shift discrete sheet:

$\left\{\begin{array}{c}da=floor(0.5\&times;Na\&times;\mathrm{\&phi;}max/\mathrm{\&pi;})\\ \mathrm{\&Delta;}row\left(i\right)=floor\&lsqb;\mathrm{\&Delta;}L\&times;\cos (i\&times;2\mathrm{\&pi;}/Na)/rowheight\&rsqb;\\ \mathrm{\&Delta;}col\left(i\right)=floor\&lsqb;\mathrm{\&Delta;}L\&times;\sin (i\&times;2\mathrm{\&pi;}/Na)/colwide\&rsqb;\end{array}\right.$

Wherein da is single extension maximum half span in the horizontal direction, floor is downward bracket function, Δ row (i) extends the child node on the i-th direction relative to father node relative displacement in line number for single, Δ col (i) extends child node on the i-th direction relative to father node relative displacement on columns for single, i=0, and 1,2,3 ... Na, rowheight are planning space line space, colwide is planning space column pitch；

When mission requirements height-precision is Nh, maximum climb/gliding angle is θ max, unmanned plane ceiling height is maxh, then calculate and storing highly skew discrete sheet:

$\left\{\begin{array}{c}\mathrm{\&Delta;}h=floor(\max h/Nh)\\ dh=floor\&lsqb;\mathrm{\&Delta;}L\&times;tan\left(\mathrm{\&theta;}\max \right)/\mathrm{\&Delta;}h\&rsqb;\\ dish\left(k\right)=\sqrt{{\mathrm{\&Delta;L}}^2+{(k\&times;\mathrm{\&Delta;}h)}^2}\end{array}\right.$

Wherein Δ h is planning vertical separation, and dh is that single extends maximum span in the height direction, and diah (k) extends the child node at kth discrete height place on specific direction and the Euclidean distance approximation of father node for single, k=0,1,2,3 ... Nh；

Step 3: route searching

Iteration performs A* searching algorithm, until finding result path and exporting, detailed step is:

First starting point it is labeled as open node and puts into Open list, total cost estimated value f takes air line distance between starting point and impact point, during search, from Open list, take out the node of total cost estimated value f Least-cost as father node every time, if this node is impact point, then search terminates, and recalls from impact point along father node iteration, exports result path；Otherwise extend child node, father node is changed into close state；

nullDuring extension child node，Limited by the current course of unmanned plane and cornering ability，Present node has maximum 2 × da+1 child node，If impact point distance father node distance is less than single leg length Δ L，Then impact point is considered as an extra alternative child node，The height bound of each child node is subject to Terrain Elevation、Minimum liftoff safe altitude、Unmanned plane ceiling and father node unmanned plane height、The angle of pitch and single maximum height span dh restriction，If there are obstacle or threat in this child node place，Then wherein Terrain Elevation is the height after superposition threat or obstacle，This scope is carried out discretization with vertical separation Δ h，Each horizontal extension direction chooses to have the minimum height node of total cost estimated value be optimum alternative child node，If its total cost estimated value is less than total cost estimated value of the original node of this horizontal position in planning space matrix M，Then replace original node with this optimum alternate node，All be updated after node all reset to open state，Add Open list and resequence，Ensure that list internal node arranges from small to large according to total cost estimated value f all the time；

Step 4: amending plans space reuse identifies

After completing the task of step 2 and step 3, planning space is reused identifier matrixflag and is added 3, abandons all data of Open table, if any new planning tasks, jumps to step 2, waits that new task restarts.