CN101339036A - Terrain auxiliary navigation method and apparatus - Google Patents
Terrain auxiliary navigation method and apparatus Download PDFInfo
- Publication number
- CN101339036A CN101339036A CNA2008101186303A CN200810118630A CN101339036A CN 101339036 A CN101339036 A CN 101339036A CN A2008101186303 A CNA2008101186303 A CN A2008101186303A CN 200810118630 A CN200810118630 A CN 200810118630A CN 101339036 A CN101339036 A CN 101339036A
- Authority
- CN
- China
- Prior art keywords
- flight path
- algorithm
- adjusted
- optimization
- prime
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Landscapes
- Navigation (AREA)
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
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
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
0Actual 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, ζ
1=|| a
1-a
0|| θ
1, a wherein
0The position of representing 1 INS in front of this coupling.Formula (3) substitution formula (1) can be got formula (4).
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.
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).
Last estimated position is expressed as formula (8).
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
0" expression matched position last time, its scope depends on the precision of whole terrain auxiliary navigation method; P
0' be actual position; P
0Represent 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
0Be 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
0" point final matched position expression last time (is a
0), P
0" P
1Not remarkable with the relation of INS, and (ρ
1, θ
1) be the parameter when carrying out the error transfer between the two, generally to make estimated position P
1" be positioned within the possible range, i.e. round zone among Fig. 6.Therefore (ρ
1, θ
1) value general bigger, decide on thick matching precision.After this estimation flight path should have certain correlativity with the INS flight path, so (ρ
2, θ
2) 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.
β 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, σ
2) 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).
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).
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).
Step 1105: at E
iAdd joint probability on the basis of value, the calculating of matching probability as the formula (14).
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
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 ζ
1=|| a
1-a
0|| θ
1a
iBe flight path indicating positions to be adjusted; a
0Be final matched position last time;
ρ wherein
iAnd θ
iThe initial value picked at random, and (ρ
1, θ
1) scope according to thick matching precision decision; (ρ
2, θ
2) 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,
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;
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
D5) at E
iAdd joint probability on the basis of value, the reckoner of matching probability is shown
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
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 ζ
1=|| a
1-a
0|| θ
1a
iBe flight path indicating positions to be adjusted; a
0Be final matched position last time;
ρ wherein
iAnd θ
iThe initial value picked at random, and (ρ
1, θ
1) scope according to thick matching precision decision; (ρ
2, θ
2) 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,
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;
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
D5) at E
iAdd joint probability on the basis of value, the reckoner of matching probability is shown
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.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN2008101186303A CN101339036B (en) | 2008-08-20 | 2008-08-20 | Terrain auxiliary navigation method and apparatus |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN2008101186303A CN101339036B (en) | 2008-08-20 | 2008-08-20 | Terrain auxiliary navigation method and apparatus |
Publications (2)
Publication Number | Publication Date |
---|---|
CN101339036A true CN101339036A (en) | 2009-01-07 |
CN101339036B CN101339036B (en) | 2010-06-23 |
Family
ID=40213166
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN2008101186303A Expired - Fee Related CN101339036B (en) | 2008-08-20 | 2008-08-20 | Terrain auxiliary navigation method and apparatus |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN101339036B (en) |
Cited By (39)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101872020A (en) * | 2010-06-18 | 2010-10-27 | 华南理工大学 | Underwater moving object location and navigation method and device based on frequency spectrum transformation |
CN102221363A (en) * | 2011-04-12 | 2011-10-19 | 东南大学 | Fault-tolerant combined method of strapdown inertial integrated navigation system for underwater vehicles |
CN102278986A (en) * | 2011-06-21 | 2011-12-14 | 海华电子企业(中国)有限公司 | Optimization method for electronic marine chart ship route design |
CN102298152A (en) * | 2010-06-23 | 2011-12-28 | 爱信艾达株式会社 | Track information generating device, track information generating method, and computer-readable storage medium |
CN102426018A (en) * | 2011-08-19 | 2012-04-25 | 北京航空航天大学 | Terrain auxiliary navigation method based on mixture of terrain contour matching (TERCOM) algorithm and particle filtering |
CN102809376A (en) * | 2012-08-06 | 2012-12-05 | 哈尔滨工程大学 | Isoline-based assistant navigation positioning method |
CN102981160A (en) * | 2012-11-08 | 2013-03-20 | 中国兵器科学研究院 | Method and device for ascertaining aerial target track |
CN103047983A (en) * | 2013-01-14 | 2013-04-17 | 哈尔滨工程大学 | Method for navigating underwater robot by matching surface terrain |
CN103542851A (en) * | 2013-11-04 | 2014-01-29 | 东南大学 | Underwater vehicle aided navigation positioning method based on underwater topography elevation database |
CN103743402A (en) * | 2014-01-03 | 2014-04-23 | 东南大学 | Underwater intelligent self-adapted terrain matching method based on terrain information amount |
CN104154911A (en) * | 2014-08-12 | 2014-11-19 | 东南大学 | Rotation invariant submarine topography two-dimensional matching aided navigation method |
CN104677359A (en) * | 2015-03-06 | 2015-06-03 | 施浒立 | Method for predicating and forecasting running track of mobile carrier |
CN105116785A (en) * | 2015-06-26 | 2015-12-02 | 北京航空航天大学 | Multi-platform remote robot general control system |
CN106020212A (en) * | 2016-05-12 | 2016-10-12 | 哈尔滨工程大学 | Navigation switching system of UUV during submarine topography tracking process and switching method thereof |
CN104316932B (en) * | 2014-11-05 | 2016-12-07 | 哈尔滨工程大学 | A kind of UUV approaches fixed high navigation system and the sailing method of seabed operation |
CN103995968B (en) * | 2014-05-22 | 2017-01-25 | 合肥工业大学 | Device and method for predicting ground target motion trail of unmanned aerial vehicle |
CN106530838A (en) * | 2016-10-25 | 2017-03-22 | 合肥飞友网络科技有限公司 | Aircraft flight path jump removing method |
CN106885576A (en) * | 2017-02-22 | 2017-06-23 | 哈尔滨工程大学 | A kind of AUV course-line deviation methods of estimation based on multiple spot terrain match positioning |
CN107367276A (en) * | 2017-06-08 | 2017-11-21 | 北京航空航天大学 | A kind of INS profile constraints series terrain matching algorithm |
CN107525502A (en) * | 2017-07-03 | 2017-12-29 | 中国空间技术研究院 | A kind of method for improving submarine navigation device inertia terrain match navigation mean accuracy |
CN107643082A (en) * | 2017-09-05 | 2018-01-30 | 东南大学 | Multipath Parallel I CCP underwater terrain matching methods based on multi-beam |
CN107727096A (en) * | 2017-09-15 | 2018-02-23 | 哈尔滨工程大学 | AUV terrain match localization methods based on the screening of effective node |
CN109409292A (en) * | 2018-10-26 | 2019-03-01 | 西安电子科技大学 | The heterologous image matching method extracted based on fining characteristic optimization |
CN109883320A (en) * | 2019-01-22 | 2019-06-14 | 湖南土流信息有限公司 | A kind of Land area measure method and system |
CN109974692A (en) * | 2019-03-21 | 2019-07-05 | 北京控制工程研究所 | A kind of hidden environment astronomical positioning system and method based on neutrino signal |
CN107167126B (en) * | 2017-03-31 | 2019-09-20 | 大鹏高科(武汉)智能装备有限公司 | A kind of autonomous type underwater robot Combinated navigation method and system |
CN110428451A (en) * | 2019-08-15 | 2019-11-08 | 中国地质大学(北京) | Topographic map and GPS device are subjected to matched operating method using GPS track |
CN110617819A (en) * | 2019-10-17 | 2019-12-27 | 国营芜湖机械厂 | Unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning |
CN111272176A (en) * | 2020-03-12 | 2020-06-12 | 中国人民解放军61540部队 | Submarine navigation method and system combining multi-beam sounding |
CN111896002A (en) * | 2020-07-22 | 2020-11-06 | 北京航空航天大学 | Method and system for online planning and adaptability analysis of terrain-assisted navigation matching area |
CN112584316A (en) * | 2019-09-30 | 2021-03-30 | 沈阳美行科技有限公司 | Indoor driving positioning method, device, equipment and storage medium |
CN113252072A (en) * | 2021-02-02 | 2021-08-13 | 中国人民解放军海军大连舰艇学院 | Digital water depth model navigable capability assessment method based on ring window |
CN113252038A (en) * | 2021-05-06 | 2021-08-13 | 西北工业大学 | Course planning terrain auxiliary navigation method based on particle swarm optimization |
CN113280811A (en) * | 2021-05-07 | 2021-08-20 | 北京航空航天大学 | Self-adaptive terrain-assisted inertial navigation method supporting multi-resolution terrain data |
CN113532438A (en) * | 2021-07-23 | 2021-10-22 | 东南大学 | Improved ICCP terrain matching method under large initial positioning error |
CN113686336A (en) * | 2021-06-30 | 2021-11-23 | 中国空间技术研究院 | Method for improving underwater navigation precision based on grid topological structure iteration optimal ring domain point |
CN114111798A (en) * | 2021-12-07 | 2022-03-01 | 东南大学 | Improved ICCP (integrated circuit chip control protocol) method based on affine factor compensation |
CN114623822A (en) * | 2022-05-12 | 2022-06-14 | 东南大学 | Multi-beam underwater terrain combination matching method based on inertia constraint |
CN115373419A (en) * | 2022-08-23 | 2022-11-22 | 中国人民解放军陆军炮兵防空兵学院 | Superlow-altitude aircraft reconnaissance monitoring method and device |
-
2008
- 2008-08-20 CN CN2008101186303A patent/CN101339036B/en not_active Expired - Fee Related
Cited By (69)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101872020B (en) * | 2010-06-18 | 2012-10-31 | 华南理工大学 | Underwater moving object location and navigation method and device based on frequency spectrum transformation |
CN101872020A (en) * | 2010-06-18 | 2010-10-27 | 华南理工大学 | Underwater moving object location and navigation method and device based on frequency spectrum transformation |
US8793090B2 (en) | 2010-06-23 | 2014-07-29 | Aisin Aw Co., Ltd. | Track information generating device, track information generating method, and computer-readable storage medium |
CN102298152A (en) * | 2010-06-23 | 2011-12-28 | 爱信艾达株式会社 | Track information generating device, track information generating method, and computer-readable storage medium |
US9273968B2 (en) | 2010-06-23 | 2016-03-01 | Aisin Aw Co., Ltd. | Track information generating device, track information generating method, and computer-readable storage medium |
CN102298152B (en) * | 2010-06-23 | 2015-04-01 | 爱信艾达株式会社 | Track information generating device, track information generating method |
US8843340B2 (en) | 2010-06-23 | 2014-09-23 | Aisin Aw Co., Ltd. | Track information generating device, track information generating method, and computer-readable storage medium |
CN102221363A (en) * | 2011-04-12 | 2011-10-19 | 东南大学 | Fault-tolerant combined method of strapdown inertial integrated navigation system for underwater vehicles |
CN102221363B (en) * | 2011-04-12 | 2012-12-19 | 东南大学 | Fault-tolerant combined method of strapdown inertial integrated navigation system for underwater vehicles |
CN102278986A (en) * | 2011-06-21 | 2011-12-14 | 海华电子企业(中国)有限公司 | Optimization method for electronic marine chart ship route design |
CN102426018A (en) * | 2011-08-19 | 2012-04-25 | 北京航空航天大学 | Terrain auxiliary navigation method based on mixture of terrain contour matching (TERCOM) algorithm and particle filtering |
CN102426018B (en) * | 2011-08-19 | 2014-01-01 | 北京航空航天大学 | Terrain auxiliary navigation method based on mixture of terrain contour matching (TERCOM) algorithm and particle filtering |
CN102809376A (en) * | 2012-08-06 | 2012-12-05 | 哈尔滨工程大学 | Isoline-based assistant navigation positioning method |
CN102809376B (en) * | 2012-08-06 | 2015-02-25 | 哈尔滨工程大学 | Isoline-based assistant navigation positioning method |
CN102981160B (en) * | 2012-11-08 | 2014-05-07 | 中国兵器科学研究院 | Method and device for ascertaining aerial target track |
CN102981160A (en) * | 2012-11-08 | 2013-03-20 | 中国兵器科学研究院 | Method and device for ascertaining aerial target track |
CN103047983B (en) * | 2013-01-14 | 2016-03-09 | 哈尔滨工程大学 | The face terrain match air navigation aid of underwater robot |
CN103047983A (en) * | 2013-01-14 | 2013-04-17 | 哈尔滨工程大学 | Method for navigating underwater robot by matching surface terrain |
CN103542851A (en) * | 2013-11-04 | 2014-01-29 | 东南大学 | Underwater vehicle aided navigation positioning method based on underwater topography elevation database |
CN103542851B (en) * | 2013-11-04 | 2016-03-23 | 东南大学 | A kind of submarine navigation device assisting navigation localization method based on underwater topography elevation database |
CN103743402A (en) * | 2014-01-03 | 2014-04-23 | 东南大学 | Underwater intelligent self-adapted terrain matching method based on terrain information amount |
CN103743402B (en) * | 2014-01-03 | 2016-06-08 | 东南大学 | A kind of underwater intelligent self adaptation Approach of Terrain Matching of topographic information based amount |
CN103995968B (en) * | 2014-05-22 | 2017-01-25 | 合肥工业大学 | Device and method for predicting ground target motion trail of unmanned aerial vehicle |
CN104154911A (en) * | 2014-08-12 | 2014-11-19 | 东南大学 | Rotation invariant submarine topography two-dimensional matching aided navigation method |
CN104154911B (en) * | 2014-08-12 | 2017-08-25 | 东南大学 | A kind of sea-floor relief two dimension matching auxiliary navigation method with rotational invariance |
CN104316932B (en) * | 2014-11-05 | 2016-12-07 | 哈尔滨工程大学 | A kind of UUV approaches fixed high navigation system and the sailing method of seabed operation |
CN104677359B (en) * | 2015-03-06 | 2017-06-30 | 施浒立 | A kind of method of dynamic carrier running orbit prediction |
CN104677359A (en) * | 2015-03-06 | 2015-06-03 | 施浒立 | Method for predicating and forecasting running track of mobile carrier |
CN105116785B (en) * | 2015-06-26 | 2018-08-24 | 北京航空航天大学 | A kind of multi-platform tele-robotic general-purpose control system |
CN105116785A (en) * | 2015-06-26 | 2015-12-02 | 北京航空航天大学 | Multi-platform remote robot general control system |
CN106020212A (en) * | 2016-05-12 | 2016-10-12 | 哈尔滨工程大学 | Navigation switching system of UUV during submarine topography tracking process and switching method thereof |
CN106020212B (en) * | 2016-05-12 | 2018-12-25 | 哈尔滨工程大学 | Navigation switching system and switching method during a kind of tracking of UUV sea-floor relief |
CN106530838A (en) * | 2016-10-25 | 2017-03-22 | 合肥飞友网络科技有限公司 | Aircraft flight path jump removing method |
CN106530838B (en) * | 2016-10-25 | 2019-04-09 | 飞友科技有限公司 | A kind of method of aircraft flight profiles jump removal |
CN106885576A (en) * | 2017-02-22 | 2017-06-23 | 哈尔滨工程大学 | A kind of AUV course-line deviation methods of estimation based on multiple spot terrain match positioning |
CN106885576B (en) * | 2017-02-22 | 2020-02-14 | 哈尔滨工程大学 | AUV (autonomous Underwater vehicle) track deviation estimation method based on multipoint terrain matching positioning |
CN107167126B (en) * | 2017-03-31 | 2019-09-20 | 大鹏高科(武汉)智能装备有限公司 | A kind of autonomous type underwater robot Combinated navigation method and system |
CN107367276B (en) * | 2017-06-08 | 2022-02-18 | 北京航空航天大学 | Terrain matching algorithm for INS (inertial navigation System) track constraint sequence |
CN107367276A (en) * | 2017-06-08 | 2017-11-21 | 北京航空航天大学 | A kind of INS profile constraints series terrain matching algorithm |
CN107525502A (en) * | 2017-07-03 | 2017-12-29 | 中国空间技术研究院 | A kind of method for improving submarine navigation device inertia terrain match navigation mean accuracy |
CN107643082A (en) * | 2017-09-05 | 2018-01-30 | 东南大学 | Multipath Parallel I CCP underwater terrain matching methods based on multi-beam |
CN107643082B (en) * | 2017-09-05 | 2020-03-31 | 东南大学 | Multipath parallel ICCP underwater terrain matching method based on multiple beams |
CN107727096A (en) * | 2017-09-15 | 2018-02-23 | 哈尔滨工程大学 | AUV terrain match localization methods based on the screening of effective node |
CN109409292A (en) * | 2018-10-26 | 2019-03-01 | 西安电子科技大学 | The heterologous image matching method extracted based on fining characteristic optimization |
CN109409292B (en) * | 2018-10-26 | 2021-09-03 | 西安电子科技大学 | Heterogeneous image matching method based on refined feature optimization extraction |
CN109883320A (en) * | 2019-01-22 | 2019-06-14 | 湖南土流信息有限公司 | A kind of Land area measure method and system |
CN109974692B (en) * | 2019-03-21 | 2021-08-10 | 北京控制工程研究所 | Hidden environment astronomical positioning system and method based on Mitsui signal |
CN109974692A (en) * | 2019-03-21 | 2019-07-05 | 北京控制工程研究所 | A kind of hidden environment astronomical positioning system and method based on neutrino signal |
CN110428451A (en) * | 2019-08-15 | 2019-11-08 | 中国地质大学(北京) | Topographic map and GPS device are subjected to matched operating method using GPS track |
CN112584316B (en) * | 2019-09-30 | 2024-02-06 | 沈阳美行科技股份有限公司 | Indoor driving positioning method, device, equipment and storage medium |
CN112584316A (en) * | 2019-09-30 | 2021-03-30 | 沈阳美行科技有限公司 | Indoor driving positioning method, device, equipment and storage medium |
CN110617819B (en) * | 2019-10-17 | 2022-09-30 | 国营芜湖机械厂 | Unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning |
CN110617819A (en) * | 2019-10-17 | 2019-12-27 | 国营芜湖机械厂 | Unmanned aerial vehicle terrain auxiliary navigation method based on ant colony algorithm path planning |
CN111272176B (en) * | 2020-03-12 | 2021-07-20 | 中国人民解放军61540部队 | Submarine navigation method and system combining multi-beam sounding |
CN111272176A (en) * | 2020-03-12 | 2020-06-12 | 中国人民解放军61540部队 | Submarine navigation method and system combining multi-beam sounding |
CN111896002A (en) * | 2020-07-22 | 2020-11-06 | 北京航空航天大学 | Method and system for online planning and adaptability analysis of terrain-assisted navigation matching area |
CN111896002B (en) * | 2020-07-22 | 2023-05-12 | 北京航空航天大学 | Terrain-assisted navigation matching area online planning and suitability analysis method and system |
CN113252072A (en) * | 2021-02-02 | 2021-08-13 | 中国人民解放军海军大连舰艇学院 | Digital water depth model navigable capability assessment method based on ring window |
CN113252038B (en) * | 2021-05-06 | 2022-10-28 | 西北工业大学 | Course planning terrain auxiliary navigation method based on particle swarm optimization |
CN113252038A (en) * | 2021-05-06 | 2021-08-13 | 西北工业大学 | Course planning terrain auxiliary navigation method based on particle swarm optimization |
CN113280811A (en) * | 2021-05-07 | 2021-08-20 | 北京航空航天大学 | Self-adaptive terrain-assisted inertial navigation method supporting multi-resolution terrain data |
CN113686336A (en) * | 2021-06-30 | 2021-11-23 | 中国空间技术研究院 | Method for improving underwater navigation precision based on grid topological structure iteration optimal ring domain point |
CN113686336B (en) * | 2021-06-30 | 2024-05-14 | 中国空间技术研究院 | Method for improving underwater navigation precision based on grid topological structure iterative optimal ring domain point |
CN113532438A (en) * | 2021-07-23 | 2021-10-22 | 东南大学 | Improved ICCP terrain matching method under large initial positioning error |
CN113532438B (en) * | 2021-07-23 | 2023-12-05 | 东南大学 | Improved ICCP terrain matching method under large initial positioning error |
CN114111798A (en) * | 2021-12-07 | 2022-03-01 | 东南大学 | Improved ICCP (integrated circuit chip control protocol) method based on affine factor compensation |
CN114623822A (en) * | 2022-05-12 | 2022-06-14 | 东南大学 | Multi-beam underwater terrain combination matching method based on inertia constraint |
CN114623822B (en) * | 2022-05-12 | 2022-08-02 | 东南大学 | Multi-beam underwater terrain combination matching method based on inertia constraint |
CN115373419A (en) * | 2022-08-23 | 2022-11-22 | 中国人民解放军陆军炮兵防空兵学院 | Superlow-altitude aircraft reconnaissance monitoring method and device |
Also Published As
Publication number | Publication date |
---|---|
CN101339036B (en) | 2010-06-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN101339036B (en) | Terrain auxiliary navigation method and apparatus | |
CN109556632B (en) | INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering | |
US9404754B2 (en) | Autonomous range-only terrain aided navigation | |
CN108362281B (en) | Long-baseline underwater submarine matching navigation method and system | |
AU640091B1 (en) | Autonomous precision weapon delivery using synthetic array radar | |
CN106052688B (en) | Inertial navigation system speed accumulation error correcting method based on terrain contour matching | |
CN110057354B (en) | Geomagnetic matching navigation method based on declination correction | |
CN109813311A (en) | A kind of unmanned plane formation collaborative navigation method | |
CN108426576B (en) | Aircraft path planning method and system based on identification point visual navigation and SINS | |
CN109341679A (en) | A kind of smart machine air navigation aid and navigation system | |
CN111426320B (en) | Vehicle autonomous navigation method based on image matching/inertial navigation/milemeter | |
CN110487276B (en) | Sampling vector matching positioning method based on correlation analysis | |
CN103674021A (en) | Integrated navigation system and method based on SINS (Strapdown Inertial Navigation System) and star sensor | |
CN108151737A (en) | A kind of unmanned plane bee colony collaborative navigation method under the conditions of the mutual observed relationships of dynamic | |
CN113739795B (en) | Underwater synchronous positioning and mapping method based on polarized light/inertia/vision integrated navigation | |
CN113252038B (en) | Course planning terrain auxiliary navigation method based on particle swarm optimization | |
CN110132281B (en) | Underwater high-speed target high-precision autonomous acoustic navigation method based on inquiry response mode | |
CN110849360B (en) | Distributed relative navigation method for multi-machine collaborative formation flight | |
Dumble et al. | Efficient terrain-aided visual horizon based attitude estimation and localization | |
CN114111767B (en) | Method for optimizing line design line type based on multi-information fusion | |
CN102168979A (en) | Isoline matching method for passive navigation based on triangular constraint model | |
Wang et al. | Land vehicle navigation using odometry/INS/vision integrated system | |
Bergman | Bayesian inference in terrain navigation | |
CN109100750A (en) | A kind of constellation navigation sensor dispatching method based on adaptive weight estimation | |
CN115560757B (en) | Unmanned aerial vehicle direct positioning correction method based on neural network under random attitude error condition |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20100623 Termination date: 20140820 |
|
EXPY | Termination of patent right or utility model |