CN105953799A - Route planning method of underwater vehicle in gravitational field adaption area based on entropy method - Google Patents

Abstract

The invention discloses a route planning method of underwater vehicle in gravitational field adaption area based on entropy method. The method includes: 1, dividing the known adaption area into several grids; 2, calculating gravitational field standard deviation, gravitational field longitude and latitude absolute roughness, gravitational field longitude and latitude correlation coefficient, and gravitational field longitude and latitude aspect for each node; 3, determining the weight of the parameters by using entropy method, multiplying each parameter and the weight of parameter, and adding products to obtain an integrated gravitational characteristic parameter; 4, determining a threshold value T of the integrated gravitational characteristic parameter; 5, calculating the average value of the integrated gravitational characteristic parameter of all the points of the adaption area edge in each local area, selecting the edge point having the largest average value as a starting point nstart of the route, and setting searching step; 6, searching next route point beginning with the starting point nstart and using the searching step as a searching radius until the terminal point; and 7, connecting all the route points from the starting point to the terminal point. The method can overcome the problem that the route deviates the area where has rich gravitational characteristic information.

Description

Underwater vehicle path planning method in gravitational field adaptation district based on Information Entropy

Technical field

The invention belongs to the technical field of Method in Gravity Aided INS system trajectory planning, be specifically related to a kind of based on entropy Underwater vehicle path planning method in the gravitational field adaptation district of method.

Background technology

Along with Aeronautics and Astronautics detective ability significantly lifting, the strategic and non-strategy of China dive device disguised contradiction get over Come the most prominent.The big country that, land-sea numerous as island has both, "Oceanic" strategy effect in China's overall development strategy and status More and more important, underwater hiding-machine technology (such as " flood dragon number " manned underwater vehicle etc.) is indispensable in the development of marine technology , there is highly important strategic importance.China's underwater hiding-machine mainly uses inertial navigation system at present, and it can provide accurate Really, reliably course, speed and positional information, it is not necessary to emission signal, without from outer received signal, and has hidden Covering property property high, entirely autonomous.But its shortcoming is error to be accumulated in time, its positioning precision can not meet the essence of device of diving when length is navigated Degree requirement.For the economy of energy and concealed consideration, underwater hiding-machine is likely to run the long period and do not emerge, So utilizing merely inertial navigation system cannot realize the most precisely navigating as navigation system, other supplementary meanss are used to improve Navigation accuracy is the trend of a certainty.

Geophysical Field Information is the intrinsic information of the earth, is often referred to by various geophysical methods attached on earth's surface or earth's surface The information of the nearly various physical phenomenons measured.In recent years, the Wuyuan navigation of geophysical character is theoretical and method causes domestic Outer research institution and the attention of scholar, carried out terrain match airmanship, geomagnetic auxiliary navigation technology and Gravity-aided navigation Technology etc. also develop rapidly.For underwater navigation, the measurement of terrain data is relatively difficult, and earth's magnetic field is not the most stable and easy Disturbed by ferromagnetic material etc., and gravitational field is the most stable and gravimetric data is prone to measure, therefore Method in Gravity Aided INS system The requirement of underwater hiding-machine " autonomy, in high precision, disguised " can greatly be met.

The trajectory planning of underwater hiding-machine is one of core technology of Method in Gravity Aided INS system, in especially adaptive district Trajectory planning.Trajectory planning refers to that finding one on the basis of considering underwater topography factor, threat and mission requirements etc. rises Point is to the feasible flight path of terminal, and its main purpose is to ensure that flight path can be good at correcting ins error after Gravity Matching, Improve the precision of Gravity-aided navigation.Path Planning is a lot, and conventional has Voronoi diagram method, A* algorithm etc..Voronoi Figure method is a kind of algorithm based on figure, and it is divided into two steps when trajectory planning: the first step threatens structure Voronoi according to known Figure, the border of Voronoi diagram constitutes feasible initial flight path, and second step combines certain searching algorithm and plans boat further Mark.The flight path that this method obtains is the most coarse, only just corresponds to solve threaten field from which when there is multiple threat and passes through Problem, and flight path is segmentation；A* algorithm is the heuristic search algorithm of a kind of classics, and this algorithm is typically employed in based on grid In the numerical map of lattice, it is used for solving the problem of static programming, planning typically takes distance cost and threatens the aspects such as cost Weighted sum represent actual cost, but its defect is that weights gather mainly by examination and search time can be with the increase of search volume Increase.Both approaches does not all account for the information of gravity Feature change, easily gravity matching and correlation inertial navigation is caused obstruction.

Summary of the invention

In view of this, the invention provides underwater vehicle trajectory planning in a kind of gravitational field adaptation district based on Information Entropy Method, it is possible to the problem solving the flight path deviation informative region of gravity field feature.

Realize technical scheme as follows:

Underwater vehicle path planning method in a kind of gravitational field adaptation district based on Information Entropy, comprises the following steps:

Step one, according to default resolution, several grids are divided into for known adaptive district；

Step 2, that each node in several grids is asked for its gravitational field standard deviation, gravitational field longitude is the most coarse Degree, gravitational field latitude absolute roughness, gravitational field longitude correlation coefficient, gravitational field latitude correlation coefficient, gravitational field longitude slope aspect With gravitational field latitude slope aspect；

Step 3, use Information Entropy determine the weight of above-mentioned each parameter, by each parameter and its multiplied by weight, and will Product addition obtains comprehensive gravity field feature parameter；

Step 4, change by observing hypsography degree and gravity anomaly in the region that causes of gravity field feature parameter size Degree determines threshold value T of comprehensive gravity field feature parameter；

Step 5, calculate adaptive area edge a little comprehensive gravity field feature parameter in respective regional area average Value, selects marginal point starting point n as flight path of comprehensive gravity field feature mean parameter maximum_start；And set according to selected areas Determine step-size in search；

Step 6, from starting point n of flight path_startStart, with step-size in search as search radius, big in comprehensive gravity field feature parameter The next track points of search in the region of threshold value T is until terminal；

Step 7, connection source, to all track points of terminal, obtain flight path to be planned.

Further, step 6 particularly as follows:

Step 6.1, with starting point n of flight path_startAs first current point, with current point as the center of circle, with step-length as radius Justifying, in getting rid of circle, comprehensive gravity field feature parameter is not more than the node of threshold value T, using possible as next step for remaining node in circle Track points, for next step possible track points each according to the actual information amount f (n of following formula zequin to terminal_start,n_j, n_goal), wherein, j >=1

$f(n_{start},n_j,n_{goal})=p_1\×\frac{1}{d(n_{start},n_j)+d(n_j,n_{goal})}+p_2\×g\_charact(n_{j-1},n_j)$

Wherein, n_j-1And n_jRepresent current point and next step possible track points, as j=1, n respectively_j-1For n_start, n_goalRepresent terminal, d (n_start,n_j) represent the starting point distance to next step possible track points, d (n_j,n_goal) represent next Walk the possible track points distance to terminal, g_charact (n_j-1,n_j) represent between current point and next step possible track points Gravity field feature information change amount, p₁、p₂Represent distance and the weight of gravity field feature information, p respectively₁＞ 0, p₂＞ 0；

Step 6.2, using next step the possible track points corresponding to actual information amount maximum as next track points；

Step 6.3, next track points is continued search for next track points as new current point according to step 6, work as terminal When being included in circle to be searched, next track points is defined as terminal, and terminates search.

Beneficial effect:

(1) path planning method of the present invention utilizes Information Entropy to determine the power of selected multiple gravity field feature parameters Weight so as to get comprehensive parameters can more comprehensively describe gravity Feature change；And it is determined by comprehensive gravity Feature The threshold value of parameter reduces the hunting zone of flight path, improves search efficiency.

(2) path planning method of the present invention is by selecting the starting point of flight path, it is ensured that it has preferably at the very start Gravity Matching effect；And it is determined by the step-length that flight path is searched for, make flight path both will not be absorbed in the region that gravitation information is poor, the most not The information in the abundant region of gravity field feature change can be lost, it is ensured that the matching efficiency of flight path.

(3) path planning method of the present invention combines Information Entropy and improves A* algorithm, consider at the same time starting point distance and Carrying out passing through the trajectory planning in adaptive district on the basis of middle track points gravitation information, it is right both to have avoided in the case of only considering distance Ignoring of gravitation information, turn avoid the deficiency only considering single parameter to gravitational field feature description, improve the gravity of flight path Matching rate.

Detailed description of the invention

Name embodiment, describe the present invention.

The invention provides underwater vehicle path planning method in a kind of gravitational field adaptation district based on Information Entropy, including Following steps:

Step one, choose a certain known 1 ° × 1 ° adaptive district, and to take resolution be 0.5' × 0.5', critical subset segmentation Become the grid of 120 × 120.

Step 2, take gravitational field standard deviation, gravitational field longitude and latitude absolute roughness, gravitational field longitude and latitude correlation coefficient and weight Longitude and latitude slope aspect totally 7 characteristic parameters in the field of force describe adaptive gravity Feature change, then take 3 × 3 around certain point Regional area calculates each gravity field feature parameter value the gravity field feature parameter value that it is put as this.

The Information Entropy that step 3, use propose determines the weight of several parameters selected above.Entropy be parameter index without The tolerance of sequence degree, may be used for measure given data comprised effective information and determine weight, by " entropy " Calculating determines weight, it is simply that according to the difference degree of every gravity field feature parameter, determine the weight of each index.When each evaluation object A certain desired value difference bigger time, entropy is less, effective information that this index provides is described relatively greatly, and its weight also should be relatively Greatly；Otherwise, if a certain desired value difference is less, entropy is relatively big, illustrates that the quantity of information that this index provides is less, and its weight also should be relatively Little.When certain desired value being respectively evaluated object is identical, entropy reaches maximum, it means that this index without useful information, Can remove from assessment indicator system.

By the weights of above-mentioned selected several characteristic parameters that Information Entropy determines, a comprehensive gravity field feature ginseng can be obtained Number, this parameter can more comprehensively describe the Changing Pattern of gravitational field in adaptive district, it is to avoid single parameter describes gravitational field The defect that quantity of information is not enough.

Weight mainly has a three below step to use Information Entropy to determine:

(1) raw data matrix normalization.If the raw data matrix of m evaluation index n evaluation object is A=a_ij, In the present invention, evaluation index be gravitational field standard deviation, gravitational field longitude and latitude absolute roughness, gravitational field longitude and latitude correlation coefficient and Gravitational field longitude and latitude slope aspect totally 7 characteristic parameters, evaluation object is each node in grid, by matrix A=a_ijAfter normalization Obtain R=(r_ij)_m×n.For the index that big person is excellent, normalization formula is:

$r_{ij}=\frac{a_{ij}-min\left\{a_{ij}\right\}}{max\left\{a_{ij}\right\}-\min \left\{a_{ij}\right\}}$

For the index that little person is excellent, normalization formula is:

(2) definition entropy.Having m index, n to be evaluated in the evaluation problem of object, the entropy of i-th index is:

$h_i=-k\underset{j=1}{\overset{n}{\Σ}}{f}_{ij}ln{f}_{ij}$

In formulaWherein k=1/ln (n), works as f_ijWhen=0, make f_ijlnf_ij=0.

(3) definition entropy weight

$w_i=\frac{1-h_i}{m-\underset{i=1}{\overset{m}{\Σ}}{h}_i}(0\≤w_i\≤1,\underset{1}{\overset{m}{\Σ}}{w}_i=1)$

Inside above-mentioned selected several gravity field feature parameters, gravitational field standard deviation sigma, gravitational field longitude and latitude absolute roughness r_λr_φ, gravitational field longitude and latitude slope aspect S_λS_φIt is the bigger the better, and gravity longitude and latitude field coefficient R_λR_φThe smaller the better.The most permissible Obtain a comprehensive gravity field feature parameter:

New_T=w₁·σ+w₂·r_λ+w₃·r_φ+w₄·R_λ+w₅·R_φ+w₆·S_λ+w₇·S_φ

Wherein w₁To w₇Represent the weight of each gravity field feature parameter respectively.

Step 4: to the new Parameter analysis of comprehensive gravity field feature obtained above it is recognised that work as comprehensive gravity spy The when of levying parameter less than a certain threshold value, in region, hypsography degree is less, and therefore gravity anomaly change is inconspicuous, is unfavorable for Carry out Gravity Matching, be therefore not suitable for carrying out trajectory planning；But when comprehensive gravity field feature parameter is more than a certain threshold value when, Hypsography degree overall in region is bigger, and therefore gravity anomaly change is obvious, is conducive to carrying out Gravity Matching, because of This compares the trajectory planning being appropriate to Gravity-aided navigation.So, obtain a comprehensive gravity by great many of experiments and contrast Threshold value T of characteristic parameter is 0.45.

Step 5, the starting point selecting flight path and step-size in search.Obtain a flight path passing through adaptive district it is necessary to selected The edge in adaptive district selects gravity field feature change, and the most significantly any is as starting point, to ensure that flight path can obtain from the beginning Well matching effect.The maximum average value method of one comprehensive gravity field feature parameter is proposed here: start at edges of regions point Selecting the regional area of m × n about, the size of m, n is consistent with selected step-size in search, calculates comprehensive gravity field feature parameter Local mean values, as the gravity field feature parameter value of that, and select maximum as the starting point of flight path.Expression formula is such as Under:

$new\_T(i,j)=\frac{1}{mn}\underset{k=i}{\overset{i+m}{\Σ}}\underset{l=j}{\overset{j+n}{\Σ}}new\_T(k,l)$

Step-length choose for the trajectory planning in adaptive district an also vital task, when taken step-length relatively Hour, latent device is easily ensnared into the gravity field feature more weak point of change and flight path is the most tortuous, thus strengthens navigation difficulty and increase Hours underway, so that ins error is excessive, is unfavorable for being corrected it；And when taken step-length is bigger, latent device holds the most very much Easily ignore the point that in the middle of flight path, the change of certain gravity anomaly is obvious, thus lose important information.Therefore choose one moderate Flight path step-size in search is critically important for we carry out trajectory planning.Here 5 a length of step-lengths of grid are selected.

Step 6, improvement Path Planning.Traditional A* algorithm evaluation function can be expressed as:

F (n)=g (n)+h (n)

What it represented is the actual cost of the optimal path from starting point to any point n.Here only examine at tradition A* algorithm On the basis of considering single parameter, searching route is added the parameter letter of the multiple description gravity Feature determined by Information Entropy Breath, makes flight path pass through the region that gravitation information is abundant in the case of moderate simultaneously so that Gravity-aided navigation coupling effect Fruit is improved.Path Planning expression formula in the adaptive district improved is as follows:

$f(n_{start},n_j,n_{goal})=p_1\×\frac{1}{d(n_{start},n_j)+d(n_j,n_{goal})}+p_2\×g\_charact(n_{j-1},n_j)$

Wherein, n_j-1And n_jRepresent current point and next step possible track points, as j=1, n respectively_j-1For n_start, n_goalRepresent terminal, d (n_start,n_j) represent the starting point distance to next step possible track points, d (n_j,n_goal) represent next Walk the possible track points distance to terminal, g_charact (n_j-1,n_j) represent between current point and next step possible track points Gravity field feature information change amount, g_charact=abs (new_T (n_j)-new_T(n_j-1)), and new_T (n_j) ＞ T is (here T represent the threshold value of comprehensive gravity field feature parameter), p₁、p₂Represent distance and the weight of gravity field feature information, and p respectively₁+p₂= 1。 (p₁> 0 and p₂>0)。

Using next step the possible track points corresponding to actual information amount maximum as next track points；By next flight path Put and continue search for next track points as new current point according to step 6, when terminal is included in circle to be searched, under inciting somebody to action One track points is defined as terminal, and terminates search.

Step 5 and step 6 select starting point n of flight path_startAnd search for each track points all described in step 4 Comprehensive gravity field feature parameter more than threshold value T region in carry out.

Step 7, connection source, to all track points of terminal, obtain flight path to be planned.

The Path Planning step improved is as follows:

The starting point selected according to said method being put in OPEN table, terminal is put in CLOSE table, and to select step-length be 5；

2), with starting point as initial point, step-length be radius surrounding's grid point in search for, less than comprehensive gravity field feature threshold value T Point put in CLOSE table；

3) search, in this range make the maximum point of heuristic function value as next impact point, and by impact point Put in CLOSE table；

4), using this impact point as new starting point, and with this according to step 2,3 carry out new search；

5), when including terminal in hunting zone, using terminal as next impact point, and search is terminated.

In sum, these are only presently preferred embodiments of the present invention, be not intended to limit protection scope of the present invention. All within the spirit and principles in the present invention, any modification, equivalent substitution and improvement etc. made, should be included in the present invention's Within protection domain.

Claims (2)

1. underwater vehicle path planning method in gravitational field adaptation district based on Information Entropy, it is characterised in that include following step Rapid:

Step one, according to default resolution, several grids are divided into for known adaptive district；

Step 2, each node in several grids is asked for its gravitational field standard deviation, gravitational field longitude absolute roughness, weight Field of force latitude absolute roughness, gravitational field longitude correlation coefficient, gravitational field latitude correlation coefficient, gravitational field longitude slope aspect and gravity Field latitude slope aspect；

Step 3, use Information Entropy determine the weight of above-mentioned each parameter, by each parameter and its multiplied by weight, and by product Addition obtains comprehensive gravity field feature parameter；

Step 4, by observing hypsography degree and gravity anomaly intensity of variation in the region that causes of gravity field feature parameter size Determine threshold value T of comprehensive gravity field feature parameter；

Step 5, calculate the adaptive area edge institute a little comprehensive gravity field feature mean parameter in respective regional area, select Select comprehensive gravity field feature mean parameter maximum marginal point starting point n as flight path_start；And according to selected areas setting search Step-length；

Step 6, from starting point n of flight path_startStart, with step-size in search as search radius, in comprehensive gravity field feature parameter more than threshold In the region of value T, search next one track points is until terminal；

Step 7, connection source, to all track points of terminal, obtain flight path to be planned.

2. underwater vehicle path planning method in gravitational field adaptation district based on Information Entropy as claimed in claim 1, it is special Levy and be, step 6 particularly as follows:

Step 6.1, with starting point n of flight path_startAs first current point, with current point as the center of circle, justify with step-length for radius, In getting rid of circle, comprehensive gravity field feature parameter is not more than the node of threshold value T, using remaining node in circle as next step possible flight path Point, for next step possible track points each according to the actual information amount f (n of following formula zequin to terminal_start,n_j, n_goal), wherein, j >=1

$f(n_{start},n_j,n_{goal})=p_1\×\frac{1}{d(n_{start},n_j)+d(n_j,n_{goal})}+p_2\×g\_charact(n_{j-1},n_j)$

Wherein, n_j-1And n_jRepresent current point and next step possible track points, as j=1, n respectively_j-1Forn_goalRepresent Terminal, d (n_start,n_j) represent the starting point distance to next step possible track points, d (n_j,n_goal) represent that next step is possible Track points is to the distance of terminal, g_charact (n_j-1,n_j) represent that the gravity between current point and next step possible track points is special Reference breath variable quantity, p₁、p₂Represent distance and the weight of gravity field feature information, p respectively₁＞ 0, p₂＞ 0；

Step 6.2, using next step the possible track points corresponding to actual information amount maximum as next track points；

Step 6.3, next track points is continued search for next track points according to step 6, when terminal includes as new current point Time in circle to be searched, next track points is defined as terminal, and terminates search.