CN101339036A - Terrain auxiliary navigation method and apparatus - Google Patents

Abstract

The invention designs a terrain aided navigation method and a device suitable for aerial (cruise missiles, pilotless aircrafts, helicopters, etc.) or submarine (submarines, underwater robots, AUV, UUV, etc.). The method is realized by using a low-accuracy inertial navigation system, especially under the condition of relatively large initial positioning error and course error. The method comprises: using a TERCOM method and weighted average to find a rough matching position to modify inertial navigation trace and form navigation track to be adjusted; using Iterative Closest Contour Point to adjust the navigation track to be adjusted with relatively large course deviation; combining the objective function optimized for a plurality of times with the matching position and introducing probability data association method to improve the matching reliability for avoiding error matching. By finishing matching under relatively low inertial navigation accuracy, the method and the device reduce the navigation cost of the low-altitude aircrafts such as cruise missile and submarine and underwater carriers.

Description

Terrain auxiliary navigation method and equipment

Technical field

The invention provides a kind of terrain auxiliary navigation method and equipment, this method and apparatus is especially under the bigger situation of initial error, use low precision inertial navigation system (Inertial Navigation System, INS) finish coupling, realize the low-cost and high-precision navigation of cruise missile, low latitude unmanned plane, aircraft, submarine, underwater robot etc.

Background technology

In the navigation of low flyer such as cruise missile or submarine, generally need the INS of degree of precision, but high precision INS cost height, and the navigation error of high precision INS also with accumulation working time, causes INS independently to work long hours.So, need other auxiliary navigation method to revise the cumulative errors of INS, to keep the long-time high precision work of navigational system.(Global PositioningSystem GPS) waits other auxiliary navigation method, and Terrain-aided Navigation has following advantage: in low flyer, no terrain masking is suitable for nap of the earth flight or landform is sheltered with respect to GPS; In the submarine, not covered by water body influences under water.But present terrain auxiliary navigation method is still bigger to the dependence of INS precision.Therefore, a kind of terrain auxiliary navigation method under low precision INS condition of research all is of practical significance at aspects such as reducing application cost and miniaturization.

After but carrier runs to the landform navigation area, INS generally has bigger position and attitude error, thereby need use to be suitable for the quick locator meams of big initial error, terrain contour matching (Terrain Contour Matching, quick search when TERCOM) method can realize big initial position error, can find matching area roughly very soon, but that its precision is influenced by the INS precision is bigger, can't operate as normal under big flight path shape error condition; The nearest equivalent point of iteration (Iterative Closest ContourPoint, ICCP) algorithm utilizes elevation isoline information correction INS flight path, the INS flight path is drawn close to true flight path gradually, reach the purpose that reduces attitude error, thereby reduced matching process to INS precision dependence, but the ICCP algorithm needs less initial alignment error ability steady operation.The present invention fully combines the advantage of TERCOM and ICCP algorithm, have complementary advantages, can be applied in this spacelike of cruise missile and aircraft on the aircraft, also can be applicable to the Terrain-aided Navigation aspect of carrier under water such as submarine and underwater robot, be suitable for the long-time navigation under the low precision INS condition.

The basic functional principle of terrain auxiliary navigation method as shown in Figure 1, at first with the landform digitizing of matching area, and be stored in the navigational computer with the form of graticule mesh digital terrain, when carrier passes through digitized coupling landform zone, measure the height value at place, carrier position with measurement of higher degree equipment 101, obtain landform altitude sampled value/sequence corresponding to a topographic profile of carrier flight path, pass through INS 103 general scope of carrier actual position as can be known in addition, in this scope, the landform altitude value/sequence that records and the digitally Figure 104 that is stored in the navigational computer are in advance together imported in the navigational computer 102, mate calculating, thereby the position that obtains mating, again this matched position is fed back to INS103, revise the INS cumulative errors.

The course of work of TERCOM as shown in Figure 2.This figure is to be the center with the current outgoing position of INS, and 3 times of the position estimation error variance is half of long, constitutes initial search window, wherein altitude information of the vertex representation of each graticule mesh (having 17 * 17 altitude figuress among Fig. 2).Carrier moves on true flight path, also write down INS position at this moment every several graticule mesh apart from gathering an altitude figures, suppose that the elevation that adds up constantly by k forms an elevation sample sequence, supposition flight path in the reference map that all possible direction is identical, length is consistent in this elevation sample sequence and the search window is carried out correlation analysis (having 17 * 17 supposition flight paths among Fig. 2) one by one, position with best correlation promptly is confirmed as the estimation of carrier actual position, be matched position, shown in the triangle form point among Fig. 2.

TERCOM can search for and convergence rapidly on a large scale; But require INS flight path shape and course all close, too big deviation can not be arranged with true flight path, otherwise can be because the supposition flight path that searches out in the search window causes the data dislocation too greatly with true course-line deviation, matching error strengthens until dispersing.

Be ICCP algorithm basic principle figure as shown in Figure 3.The true flight path of carrier is by P ' _i(i=1,2, L W) forms, and wherein W is the length (counting) of flight path; The INS flight path is by P _iPoint is formed; In addition, sensor measurement is to local actual landform c topographical surveying _i, each altitude information is corresponding to a level line in the topomap.Because there is measuring error in INS, P _iWith P ' _iBetween have certain deviation inevitably.The thought of ICCP algorithm is exactly: P ' _iOne is positioned c _iOn the isoline or near, can make P according to certain principle so " _iBe brought to c _iOn, find optimal estimation point P " _iAnd flight path, realize error compensation to INS.This is actually a kind of optimization procedure, for by W the segment of curve formed of point, unique adjustable be that these points all are correspondingly positioned at isoline separately, and can slide on the isoline separately, finally make target function value reach minimum.With total error as objective function, promptly

$E=\underset{i=1}{\stackrel{W}{\mathrm{\Σ}}}d(x_i-x_{i-1},a_i-a_{i-1})+K\underset{i=1}{\stackrel{W}{\mathrm{\Σ}}}d(x_i,y_i)---\left(1\right)$

In the formula: E is objective function, i.e. total error; (p q) is distance between p and the q to d; x _iBe estimation point P " _iThe position, y _iBe x _iPosition from the nearest point of isoline; a _iBe the INS indicating positions; K is a stiffness coefficient.

Here think that the INS initial error is zero, as shown in Figure 3 c ₀Actual position on the level line, INS position and estimated position overlap; According to principle of optimality, the objective function of formula (1) is optimized adjustment, promptly change the position of estimation point, make the E minimum, and think that this sequence is the optimal estimation to current true flight path, promptly finishes P " _iThe estimation of sequence.

Be illustrated in figure 4 as the velocity error of carrier and the influence that deflection error was navigated by water to a step, wherein ρ _iBe bearer rate error, θ _iBe carrier course error, b _I+1Be the unit vector along the navigation direction, e _I+1For in the surface level and b _I+1The unit vector of perpendicular direction.Can obtain formula (2) by Fig. 4.

x _i＝x _i-1+(||a _i-a _i-1||+ρ _i)(cosθ _ib _i+sinθ _ie _i) (2)

Because ρ _iAnd θ _iFormula (2) is in a small amount, so can be approximated to be formula (3).

x _iBx _i-1+a _i-a _i-1+ρ _ib _i+ζ _ie _i (3)

In the formula: ζ _i=|| a _i-a _I-1|| θ _i, ζ ₁=|| a ₁-a ₀|| θ ₁, a wherein ₀The position of representing 1 INS in front of this coupling.Formula (3) substitution formula (1) can be got formula (4).

$E=\underset{i=1}{\stackrel{W}{\mathrm{\Σ}}}({\mathrm{\ρ}}_i^2+{\mathrm{\ζ}}_i^2)+K\underset{i=1}{\stackrel{W}{\mathrm{\Σ}}}{\left|\right|x_i-y_i\left|\right|}^2---\left(4\right)$

At known a _i, ρ _iAnd θ _iPrerequisite under, x _i(i=1,2, L W) can calculate according to formula (3), so x _iBe ρ _iAnd θ _iFunction.In addition, y _iBe according to x _iReckoning is come out, therefore, and y _iAlso be ρ _iAnd θ _iFunction.

Optimizing process carries out optimal treatment to formula (4) exactly, because x _iAnd y _iAll are ρ _iAnd θ _iFunction, so optimization variable is exactly ρ _iAnd θ _i, objective function is E.

Be tending towards at 0 o'clock at K,, therefore, can only flight path do not carried out any adjustment, but make objective function be tending towards optimal value by rigid transformation (rotation and translation) because any little variation of flight path all may cause target function value to change significantly.

As mentioned above, basic I CCP algorithm is suitable for the bigger situation of INS flight path form variations; But this scheme requires the INS initial error very little, and the ICCP algorithm of rigid transformation scheme requires the shape error of INS very little in addition, so present ICCP algorithm all can not satisfy the demand of practical application.

Summary of the invention

The technical matters that the present invention solves is: under the condition of big initial error and low precision INS, study a kind of terrain match method and apparatus that can reliably carry out assisting navigation, advantage is can extensive search, convergence fast, revise the flight path shape of INS, reduce the cost of navigational system, improve the precision of navigation.

According to an aspect of the present invention, provide a kind of and be suitable in the air or terrain auxiliary navigation method under water, the Terrain-aided Navigation that it can be used for big initial error and low precision INS is characterized in that comprising:

Determine a window that comprises current I NS position;

In described window, adopt the TERCOM method to calculate thick matched position;

Utilize the described INS of described thick matched position correction position, stack INS flight path shape is as flight path to be adjusted on thick matched position;

Use the ICCP algorithm that this flight path to be adjusted is adjusted, make the true flight path of course made good to be adjusted approaching.

According to a further aspect of the present invention, provide a kind of and be suitable in the air or Terrain-aided Navigation equipment under water, the Terrain-aided Navigation that it can be used for big initial error and low precision INS is characterized in that comprising:

The navigation position window is determined device, is used for determining a window that comprises current I NS position;

Thick matched position calculation element is used for adopting the TERCOM method to calculate thick matched position at described window;

Flight path generating apparatus to be adjusted is used to utilize the described INS of described thick matched position correction position, and stack INS flight path shape is as flight path to be adjusted on thick matched position;

The flight path adjusting gear is used to use the ICCP algorithm that this flight path to be adjusted is adjusted, and makes the true flight path of course made good to be adjusted approaching.

Principle of the present invention is: utilize TERCOM and weighted mean to choose thick matched position with the method that prevents the mistake coupling, improve the rapidity and the robustness of matching process, stack INS flight path shape uses the ICCP algorithm that flight path to be adjusted is drawn close towards true flight path as flight path to be adjusted on thick matched position; Utilizing isoline correction flight path to be adjusted, promptly is with elevation information correction INS flight path, utilizes simplex evolutionary method to search out minimum target function value, utilizes Probabilistic Data Association Algorithm to improve the reliability of smart coupling simultaneously, thereby obtains the Optimum Matching flight path.

The present invention's advantage compared with prior art is: correlation technique is suitable under the less situation of flight path shape error, thereby requires the INS precision higher; Kalman filter method requires the INS precision higher equally; Simple ICCP algorithm needs initial error less or the flight path form variations is little.Method of the present invention can adapt to the INS flight path of lower accuracy, can revise the INS flight path, and this is not have in the former technology.The method of rigid transformation that adopts a lot of ICCP algorithms realizes the registration of flight path to be matched and true flight path, but its application is subjected to the very big restriction of INS flight path shape error; The present invention adopts the method for non-rigid transformation, can realize any conversion of flight path, can approach the shape of true flight path more.

Description of drawings

Fig. 1 is the terrain auxiliary navigation method structure

Fig. 2 is a TERCOM method synoptic diagram

Fig. 3 is ICCP algorithm principle figure

Fig. 4 is the influence to site error of bearer rate and deflection error

Fig. 5 is ICCP algorithm principle figure used in the present invention

Fig. 6 is (ρ _i, θ _i) choose the scope synoptic diagram

Fig. 7 is twice optimization position distribution schematic diagram

Fig. 8 is method realization flow figure

Fig. 9 is thick coupling realization flow figure

Figure 10 adjusts realization flow figure for ICCP

Figure 11 is mistake matching judgment realization flow figure

Embodiment

Flow process of the present invention as shown in Figure 8.

(1) rudimentary algorithm that the TERCOM method is adopted among the present invention comprises:

Cross correlation algorithm (Cross Correlation, CC or Productcorrelation similarity measure, Prod), the normalized crosscorrelation algorithm (Normalized Product correlation similarity measure, NProd); The absolute difference algorithm (Absolute Difference, AD), difference of two squares algorithm (Square Difference, SD), the average absolute difference algorithm (MeanAbsolute Difference, MAD), the mean square difference algorithm (Mean SquareDifference, MSD); Improved absolute difference, improved difference of two squares algorithm for example utilize attenuation coefficient improved; Matching algorithm based on Haus edorff distance.

(2) the present invention at first carries out the definite thick match search window of step 801: adopting 3 σ principles, is the moving-square search window that one 6 σ * 6 σ are opened at the center with the INS position.

(3) the present invention adopts a kind of decay absolute difference method to realize thick coupling 802 as the TERCOM method.Fig. 9 is the further process flow diagram of thick coupling 802.This method is a kind of iterative calculation method, and the measurement of decay absolute difference method is designated as α AD.With reference to figure 2, step 901 is obtained the process of numerical map in the search window, and step 902 definition k single-point errors table constantly is shown then:

e _m，n(k)＝h _measure(k)-h _map(x _k+m，y _k+n) (5)

Wherein: m, n represent the position of the relative window center of graticule mesh; h _Measure(k) expression k measurement elevation constantly; h _Map(x _k+ m, y _k+ n) the height value of expression in the map; x _k, y _kWhat represent is the center of search window.

Define α AD as the formula (6), among Fig. 2, α AD is the matrix of one 17 * 17 sizes.

$\mathrm{\α}{\mathrm{AD}}_{m,n}=\left|e_{m,n}\left(k\right)\right|+\mathrm{\αg}\left|e_{m,n}(k-1)\right|+{\mathrm{\α}}^{2}g\left|e_{m,n}(k-2)\right|+{\mathrm{\α}}^{k-2}g\left|e_{m,n}\left(2\right)\right|+{\mathrm{\α}}^{k-1}g\left|e_{m,n}\left(1\right)\right|$

$=\underset{i=1}{\stackrel{k}{\mathrm{\Σ}}}{\mathrm{\α}}^{k-i}\left|e_{m,n}\left(i\right)\right|---\left(6\right)$

Step 903 is chosen t minimum among matrix α AD value, step 904 judges that whether the distribution variance of above-mentioned t some position is less than preset threshold, "Yes" then illustrates thick matching process convergence, program proceeds to step 906 to utilize the thick matched position of calculated with weighted average method, "No" then illustrates the also not convergence of thick matching process, program proceeds to step 905, exports as the location with the INS position.

If the t that meets the demands some location tables is shown [m _in _i] ^T, (i=1,2 ..., t), respective weights is β _i(k), the weight calculation formula in the step 906 is formula (7).

${\mathrm{\β}}_i\left(k\right)=\frac{1/\mathrm{\α}{\mathrm{AD}}_{m_i,n_i}}{\underset{j=1}{\stackrel{t}{\mathrm{\Σ}}}1/\mathrm{\α}{\mathrm{AD}}_{m_j,n_j}}i=\mathrm{1,2},...,t---\left(7\right)$

Last estimated position is expressed as formula (8).

$\left[\begin{array}{c}\hat{m}\\ \hat{n}\end{array}\right]=\underset{i=1}{\overset{t}{\mathrm{\Σ}}}\left[\begin{array}{c}{m}_i\\ n_i\end{array}\right]{\mathrm{\β}}_i\left(k\right)---\left(8\right)$

Step 803 is utilized thick matched position and INS flight path shape accumulative total flight path to be adjusted, whether step 907 judges that ICCP adjusts counts to accumulate and reaches W, "Yes" is then carried out ICCP and is adjusted 804, and "No" is then carried out the thick matched position of step 908 output as final matched position.

(4) employed ICCP adjusts 804 as follows:

The process of ICCP adjustment 804 as shown in figure 10 among the present invention.The ICCP method is a kind of batch processing method, need carry out the track points to be adjusted of the enough numbers of step 803 accumulative total and just can carry out ICCP adjustment 804, if also there are not enough numbers, this moment should be with thick matched position as location output (step 908); In case number reaches requirement, carry out ICCP and adjust 804.

Fig. 5 is the ICCP method after improving, P ₀" expression matched position last time, its scope depends on the precision of whole terrain auxiliary navigation method; P ₀' be actual position; P ₀Represent thick matched position.

ICCP algorithm after the improvement is: utilize thick matched position stack INS flight path shape to form flight path to be adjusted, replace the INS flight path; Still adopt the objective function shown in the formula (4), but a of this moment _i(i=1,2 ..., W) be flight path to be adjusted, a ₀Be final matched position last time.

ρ in the step 1001 _iAnd θ _iThe initial value picked at random, but owing to be subjected to the influence of thick coupling, the variation range of each point is different.As shown in Figure 6.P ₀" point final matched position expression last time (is a ₀), P ₀" P ₁Not remarkable with the relation of INS, and (ρ ₁, θ ₁) be the parameter when carrying out the error transfer between the two, generally to make estimated position P ₁" be positioned within the possible range, i.e. round zone among Fig. 6.Therefore (ρ ₁, θ ₁) value general bigger, decide on thick matching precision.After this estimation flight path should have certain correlativity with the INS flight path, so (ρ ₂, θ ₂) can choose according to 3 σ principles of INS speed and course deviation variance; Other point all is to increase a little random quantity get final product (little random number on INS drift value is big or small in short-term decide) on more preceding basis.As the formula (9).

x _iAnd y _iBe ρ _iAnd θ _iFunction, utilize initialized ρ _iAnd θ _iCan carry out step 1002, calculate estimated position x _iWith closest approach position y _iThen utilize formula (4) carry out step 1003, the calculating target function value, last ICCP algorithm is converted into the mathematical problem of multiparameter optimizing, utilize step 1004 to carry out the modified simplex evolutionary method and seek optimum solution, and in step 1005, this optimum solution is exported as the optimal location of this time through reflection, expansion and contraction.

(5) adjust in the mistake matching judgment of adjusting the result in 804 processes at ICCP, employed probabilistic data association prevents the mistake matching process as shown in figure 11.

In order to obtain global optimum, simplex optimization is carried out Q group ICCP altogether adjust, its objective function converges to E _MinPerhaps iterations surpasses threshold value T _MaxThe time, record objective function E value at this moment is as the index of coupling.

Give less E value optimization position with big probability, with than small probability, and guarantee that total probability equals 1 to bigger E value optimization position, formula (10) is seen in probability calculation first.

${\mathrm{\β}}_i=\frac{1/E_i}{\underset{k=1}{\stackrel{Q}{\mathrm{\Σ}}}(1/E_k)}---\left(10\right)$

β wherein _i(i=1,2 ..., Q) be the probability of each suboptimization position.

Step 1101: the probability of inciting somebody to action optimization position last time is designated as β _{J '}' (j '=1,2 ..., Q), the probability of this optimization position is designated as β _i(i=1,2, ..., Q), twice INS position is overlapped, can be placed on the position of two suboptimization among the figure, Figure 7 shows that the synoptic diagram that is combined into a figure, wherein total last time with each Q position of this suboptimization and last time with this INS location point, each optimization position all correspondence a numbering.

Step 1102: each optimization position of supposing this all may come from any one optimization position last time, if the INS positioning error obey N (0, σ ²) Gaussian distribution, so last time j ' of optimization position to the probability density of the i point effect of this optimization position as the formula (11).

$p\left(i\right|j^{\&prime;})={\frac{1}{\sqrt{2\mathrm{\&pi;}}\mathrm{\&sigma;}}e}^{-\frac{{\mathrm{<figure-callout\; id="2"\; label="dis"\; filenames="A20081011863000171.png"\; state="\{\{state\}\}">dis</figure-callout>}}^2}{{2\mathrm{\&sigma;}}^2}}---\left(11\right)$

Wherein dis is the distance after two optimization position of i, j ' are integrated through INS, promptly before j ' of suboptimization in this suboptimization predicted position and the distance between this optimization position i point.

Step 1103: this a Q optimization position all may come from the j ' point in the optimization position last time, can decide the conditional probability of this each point according to its probability density, as the formula (12).

$P\left(i\right|j^{\&prime;})=\frac{p\left(i\right|j^{\&prime;})}{\underset{k=1}{\stackrel{Q}{\mathrm{\&Sigma;}}}p\left(k\right|j^{\&prime;})}---\left(12\right)$

Step 1104: utilize formula (12) and total probability formula can obtain last time of the influence of Q optimization position to this order i optimization position, i.e. the joint probability of i optimization position, as the formula (13).

$P\left(i\right)=\underset{j^{\&prime;}=1}{\stackrel{Q}{\mathrm{\&Sigma;}}}{{\mathrm{\&beta;}}_{j^{\&prime;}}}^{\&prime;}\mathrm{gP}\left(i\right|j^{\&prime;})---\left(13\right)$

Step 1105: at E _iAdd joint probability on the basis of value, the calculating of matching probability as the formula (14).

${\mathrm{\&beta;}}_i=\frac{P\left(i\right)/E_i}{\underset{k=1}{\stackrel{Q}{\mathrm{\&Sigma;}}}(P\left(k\right)/E_k)}---\left(14\right)$

The optimization position of choosing maximum probability is as this final matching results.

Claims (10)

1, a kind of being suitable in the air or terrain auxiliary navigation method under water,, the Terrain-aided Navigation that it can be used for big initial error and low precision inertial navigation system is characterized in that comprising:

A) determine a window that comprises current inertial navigation position;

B) in described window, adopt the TERCOM method to calculate thick matched position;

C) utilize the described inertial navigation of described thick matched position correction position, stack inertial navigation flight path shape is as flight path to be adjusted on thick matched position;

D) use the ICCP algorithm that this flight path to be adjusted is adjusted, make the true flight path of course made good to be adjusted approaching.

2, terrain auxiliary navigation method according to claim 1 is characterized in that: comprising of employed thick matching locating method step B): cross correlation algorithm, normalized crosscorrelation algorithm, absolute difference algorithm, difference of two squares algorithm, average absolute difference algorithm, mean square difference algorithm, improved absolute difference, improved difference of two squares algorithm and/or based on the matching algorithm of Hausedorff distance.

3, terrain auxiliary navigation method according to claim 1 is characterized in that: step B) further comprise:

Big saltus step for thick matched position occurs because of the landform similarity utilizes weighted average method to obtain reliable matched position;

The calculation of parameter weights that utilize the TERCOM method to be calculated.

4, terrain auxiliary navigation method according to claim 1 is characterized in that: step D) employed ICCP algorithm further comprises:

Thick matched position stack INS flight path shape, form flight path to be adjusted;

Be positioned at c according to true flight path _iNear the isoline rule according to a pre-defined rule, makes flight path to be adjusted be brought to c _iOn, and definite optimal estimation flight path, thus realize error compensation to INS, and wherein said predetermined principle is that the represented objective function E of following formula gets minimum value

$E=\underset{i=1}{\overset{W}{\mathrm{\&Sigma;}}}({\mathrm{\&rho;}}_i^2+{\mathrm{\&zeta;}}_i^2)+K\underset{i=1}{\overset{W}{\mathrm{\&Sigma;}}}{\left|\right|x_i-y_i\left|\right|}^2$

In the formula: E is that objective function is a total error; W is counting that flight path to be adjusted comprised;

x _iBe the position of estimation point, y _iBe x _iPosition from the nearest point of isoline; K is a stiffness coefficient; ρ _iBe the bearer rate error; θ _iCourse error for carrier; ζ _i=|| a _i-a _I-1|| θ _iAnd ζ ₁=|| a ₁-a ₀|| θ ₁a _iBe flight path indicating positions to be adjusted; a ₀Be final matched position last time;

ρ wherein _iAnd θ _iThe initial value picked at random, and (ρ ₁, θ ₁) scope according to thick matching precision decision; (ρ ₂, θ ₂) choose according to 3 σ principles of INS speed, course deviation variance; Other point is chosen according to following principle

Its medium and small random number on INS in short-term drift value size decide;

X wherein _iAnd y _iBe ρ _iAnd θ _iFunction,

Wherein the ICCP algorithm is converted into the mathematical problem of multiparameter optimizing, utilizes the modified simplex evolutionary method to seek optimum matched position through reflection, expansion and contraction.

5, terrain auxiliary navigation method according to claim 1 is characterized in that: step D) employed ICCP algorithm further comprises:

D1) for realizing global optimum, adopting the stochastic sampling mode, is benchmark with flight path to be adjusted promptly, to ρ _iAnd θ _iCarry out stochastic sampling, the Q that samples altogether group is optimized initial value to this Q group then and is carried out the ICCP optimization computation respectively, and its objective function converges to E _MinPerhaps iterations surpasses threshold value T _MaxThe time, record objective function E value at this moment is as the index of coupling;

D2) as shown in the formula calculating last time j ' i point effect on the optimization position to this optimization position,

$p\left(i\right|j^{\&prime;})=\frac{1}{\sqrt{2\mathrm{\&pi;}}\mathrm{\&sigma;}}{e}^{-\frac{{\mathrm{dis}}^2}{2{\mathrm{\&sigma;}}^2}}$

Wherein dis be j ' of preceding suboptimization in this suboptimization predicted position and the distance between this optimization position i point, σ is the predicted position error variance;

D3) under this Q optimization position all may come from j ' situation in the optimization position last time, decide the conditional probability of this each point according to its probability density;

$P\left(i\right|j^{\&prime;})=\frac{p\left(i\right|j^{\&prime;})}{\underset{k=1}{\overset{Q}{\mathrm{\&Sigma;}}}p\left(k\right|j^{\&prime;})}$

D4) utilize total probability formula, obtain last time of the influence of Q optimization position, i.e. the joint probability of i optimization position, wherein β this order i optimization position _{J '}' expression is the probability of each optimization position last time

$P\left(i\right)=\underset{j^{\&prime;}=1}{\overset{Q}{\mathrm{\&Sigma;}}}{{\mathrm{\&beta;}}_{j^{\&prime;}}}^{\&prime;}\mathrm{gP}\left(i\right|j^{\&prime;})$

D5) at E _iAdd joint probability on the basis of value, the reckoner of matching probability is shown

${\mathrm{\&beta;}}_i=\frac{P\left(i\right)/E_i}{\underset{k=1}{\overset{Q}{\mathrm{\&Sigma;}}}(P\left(k\right)/E_k)}$

D6) optimization position of choosing maximum probability is as final matching results, and then order is carried out above-mentioned steps D1), D2), D3), D4), D5), thereby obtain a series of more reliable matching result.

6, a kind of being suitable in the air or Terrain-aided Navigation equipment under water,, the Terrain-aided Navigation that it can be used for big initial error and low precision inertial navigation system is characterized in that comprising:

The navigation position window is determined device, is used for determining a window that comprises current inertial navigation position;

Thick matched position calculation element is used for adopting the TERCOM method to calculate thick matched position at described window;

Flight path generating apparatus to be adjusted is used to utilize the described inertial navigation of described thick matched position correction position, and stack inertial navigation flight path shape is as flight path to be adjusted on thick matched position;

The flight path adjusting gear is used to use the ICCP algorithm that this flight path to be adjusted is adjusted, and makes the true flight path of course made good to be adjusted approaching.

7, Terrain-aided Navigation equipment according to claim 6 is characterized in that:

Comprising of described employed thick matching locating method: cross correlation algorithm, normalized crosscorrelation algorithm, absolute difference algorithm, difference of two squares algorithm, average absolute difference algorithm, mean square difference algorithm, improved absolute difference, improved difference of two squares algorithm and/or based on the matching algorithm of Hausedorff distance.

8, Terrain-aided Navigation equipment according to claim 6 is characterized in that: described thick matched position calculation element further comprises:

The saltus step retouch is used for the landform similarity of big saltus step occur because of to(for) thick matched position, utilizes weighted average method to obtain reliable matched position;

The weights calculating section is used to the calculation of parameter weights that utilize the TERCOM method to be calculated.

9, Terrain-aided Navigation equipment according to claim 6 is characterized in that: employed ICCP algorithm further comprises:

Thick matched position stack INS flight path shape, form flight path to be adjusted;

Be positioned at c according to true flight path _iNear the isoline rule according to a pre-defined rule, makes flight path to be adjusted be brought to c _iOn, and definite optimal estimation flight path, thus realize error compensation to INS, and wherein said predetermined principle is that the represented objective function E of following formula gets minimum value

$E=\underset{i=1}{\overset{W}{\mathrm{\&Sigma;}}}({\mathrm{\&rho;}}_i^2+{\mathrm{\&zeta;}}_i^2)+K\underset{i=1}{\overset{W}{\mathrm{\&Sigma;}}}{\left|\right|x_i-y_i\left|\right|}^2$

In the formula: E is that objective function is a total error; W is counting that flight path to be adjusted comprised; x _iBe the position of estimation point, y _iBe x _iPosition from the nearest point of isoline; K is a stiffness coefficient; ρ _iBe the bearer rate error; θ _iCourse error for carrier; ζ _i=|| a _i-a _I-1|| θ _iAnd ζ ₁=|| a ₁-a ₀|| θ ₁a _iBe flight path indicating positions to be adjusted; a ₀Be final matched position last time;

ρ wherein _iAnd θ _iThe initial value picked at random, and (ρ ₁, θ ₁) scope according to thick matching precision decision; (ρ ₂, θ ₂) choose according to 3 σ principles of INS speed, course deviation variance; Other point is chosen according to following principle

Its medium and small random number on INS in short-term drift value size decide;

X wherein _iAnd y _iBe ρ _iAnd θ _iFunction,

Wherein the ICCP algorithm is converted into the mathematical problem of multiparameter optimizing, utilizes the modified simplex evolutionary method to seek optimum matched position through reflection, expansion and contraction.

10, Terrain-aided Navigation equipment according to claim 6 is characterized in that the employed ICCP algorithm of described flight path adjusting gear further comprises:

D1) for realizing global optimum, adopting the stochastic sampling mode, is benchmark with flight path to be adjusted promptly, to ρ _i, θ _iCarry out stochastic sampling, the Q that samples altogether group is optimized initial value to this Q group then and is carried out the ICCP optimization computation respectively, and its objective function converges to E _MinPerhaps iterations surpasses threshold value T _MaxThe time, record objective function E value at this moment is as the index of coupling;

D2) as shown in the formula calculating last time j ' i point effect on the optimization position to this optimization position,

$p\left(i\right|j^{\&prime;})=\frac{1}{\sqrt{2\mathrm{\&pi;}}\mathrm{\&sigma;}}{e}^{-\frac{{\mathrm{dis}}^2}{2{\mathrm{\&sigma;}}^2}}$

Wherein dis be j ' of preceding suboptimization in this suboptimization predicted position and the distance between this optimization position i point, σ is the predicted position error variance;

D3) under this Q optimization position all may come from j ' situation in the optimization position last time, decide the conditional probability of this each point according to its probability density;

$P\left(i\right|j^{\&prime;})=\frac{p\left(i\right|j^{\&prime;})}{\underset{k=1}{\overset{Q}{\mathrm{\&Sigma;}}}p\left(k\right|j^{\&prime;})}$

D4) utilize total probability formula, obtain last time of the influence of Q optimization position, i.e. the joint probability of i optimization position, wherein β this order i optimization position _{J '}' expression is the probability of each optimization position last time

$P\left(i\right)=\underset{j^{\&prime;}=1}{\overset{Q}{\mathrm{\&Sigma;}}}{{\mathrm{\&beta;}}_{j^{\&prime;}}}^{\&prime;}\mathrm{gP}\left(i\right|j^{\&prime;})$

D5) at E _iAdd joint probability on the basis of value, the reckoner of matching probability is shown

${\mathrm{\&beta;}}_i=\frac{P\left(i\right)/E_i}{\underset{k=1}{\overset{Q}{\mathrm{\&Sigma;}}}(P\left(k\right)/E_k)}$

D6) optimization position of choosing maximum probability is as final matching results, and then order is carried out above-mentioned steps D1), D2), D3), D4), D5), thereby obtain a series of more reliable matching result.

Images