CN104075715B - A kind of underwater navigation localization method of Combining with terrain and environmental characteristic - Google Patents

A kind of underwater navigation localization method of Combining with terrain and environmental characteristic Download PDF

Info

Publication number
CN104075715B
CN104075715B CN201410320791.6A CN201410320791A CN104075715B CN 104075715 B CN104075715 B CN 104075715B CN 201410320791 A CN201410320791 A CN 201410320791A CN 104075715 B CN104075715 B CN 104075715B
Authority
CN
China
Prior art keywords
mrow
moment
environmental characteristic
msub
mtd
Prior art date
Application number
CN201410320791.6A
Other languages
Chinese (zh)
Other versions
CN104075715A (en
Inventor
徐晓苏
汤郡郡
李佩娟
张涛
岳增阳
Original Assignee
东南大学
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by 东南大学 filed Critical 东南大学
Priority to CN201410320791.6A priority Critical patent/CN104075715B/en
Publication of CN104075715A publication Critical patent/CN104075715A/en
Application granted granted Critical
Publication of CN104075715B publication Critical patent/CN104075715B/en

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20Instruments for performing navigational calculations

Abstract

The invention discloses a kind of Combining with terrain and the underwater navigation localization method of environmental characteristic, judge whether each sub-regions can match in the planning flight path of underwater hiding-machine according to landform information content, positioning is realized using terrain aided inertial navigation system if it can match, aids in main inertial navigation system to realize using synchronous positioning and composition algorithm if it can not match and positions.The present invention by the calculating to navigation area terrain information amount, by sea-floor relief be divided into landform can matching area and landform can not matching area.For different regions, the site error amendment to main inertial navigation system is realized using different navigation algorithms, with higher independence.

Description

A kind of underwater navigation localization method of Combining with terrain and environmental characteristic

Technical field

The present invention relates to underwater navigation technical field, specifically design one kind and disclosure satisfy that underwater hiding-machine is high for a long time The method of precision Camera calibration requirement.

Background technology

Inertial navigation system does not need any external information, will not be to any information of external radiation, only by inertial navigation system yet System inherently can under all weather conditions, in the world with carry out continuous three-dimensional localization and leading in any media environment Boat, it is this be provided simultaneously with independence, particular advantages that are disguised and can obtaining the complete movable information of carrier are such as wireless conductances Other navigation system such as boat, satellite navigation and celestial navigation are incomparable.But, inertial navigation system greatest weakness is that it is Error of uniting is with time integral, and the time is longer, and error is bigger.In order to ensure the safe navigation and the accurate hair of weapon of underwater hiding-machine Penetrate, it is necessary to enter the adjustment and correction of line period to inertial navigation system using extraneous positional information.

Models in Terrain Aided Navigation (Terrain-Aided Navigation, TAN), is substantially by inertial navigation system (providing real-time figure) is constituted with sensor (real-time figure is contacted and tie with reference map) and numerical map (providing reference map) Integrated navigation system, it is used as a kind of high-precision navigator fix technology, it is only necessary to which terrain information content is enriched and increased enough deposits The capacity of reservoir can just improve navigation accuracy nearly an order of magnitude, reach more than ten meters of positioning precision.Terrain-aided Navigation has There is autonomous, hidden, continuous, all weather operations, navigation positioning error, not with the advantage of time integral, is that underwater hiding-machine is preferably auxiliary Assistant director of a film or play's boat positioning means.

The typical Models in Terrain Aided Navigation based on ICCP algorithms can be with priori topographic map and topography variation is bright Aobvious marine site, corrects the site error of main inertial navigation system.However, Terrain-aided Navigation needs the topographic map of priori, and It is required that there is obvious topography variation, not yet surveyed and drawn for those or topography variation is excessively gentle, the unconspicuous sea of features of terrain Domain, it is very difficult that main inertial navigation system is reduced with terrain auxiliary navigation method with the error of time integral.

Now, using the observation to other submerged structures of feature under water, shipwreck with time stability etc., adopt With based on marine environment feature synchronous superposition (Simultaneous Localization and Mapping, SLAM) algorithm is used as the alternative of Terrain-aided Navigation, to correct the position of underwater hiding-machine in real time, reduction navigation error Accumulation, improves the positioning precision of navigation system.

The content of the invention

The technical problem to be solved:In view of the shortcomings of the prior art, the present invention proposes a kind of Combining with terrain and environmental characteristic Underwater navigation localization method, for the seabed priori topographic map non-availability either marine site do not enriched of terrain information amount, it is impossible to Technical problem of the main inertial navigation system with the site error of time integral is reduced using Models in Terrain Aided Navigation.

Technical scheme:In order to solve the above technical problems, the present invention uses following technical scheme:

A kind of underwater navigation localization method of Combining with terrain and environmental characteristic, underwater hiding-machine is judged according to landform information content Whether each sub-regions can be matched in planning flight path, and positioning is realized using terrain aided inertial navigation system if it can match, if It can not match and then realize and position using synchronous positioning and the main inertial navigation system of composition algorithm auxiliary.

Terrain auxiliary navigation method have the advantages that it is autonomous, hidden, terrain information amount enrich match marine site, typically It is to be based on ICCP algorithms, the error of inertial navigation system is corrected according to priori topographic map, modification method and makeover process are Prior art, can be very good the navigator fix for aiding in main inertial navigation system to realize underwater hiding-machine;And can not be matched in landform Region, synchronous positioning and composition algorithm, have played itself to be observed the environmental characteristic with time stability and have determined to realize Position, and the characteristics of correct main inertial navigation system site error, effectively compensate for the deficiency of Models in Terrain Aided Navigation;Two kinds Method is used cooperatively so that the navigator fix of underwater hiding-machine has more preferable independence and accuracy.

Further, in the present invention, judge what whether each sub-regions in the planning flight path of underwater hiding-machine can match Method is as follows:

Step 1.1:Sea-floor relief elevation is dived under water using the model split of grid matrix into polylith landform candidate region By wherein L blocks landform candidate region on the planning flight path of device, set the longitude and latitude span of a certain piece of landform candidate region as M × N grids, and grid point coordinates is that the landform altitude value at (i, j) place is height (i, j), i=1,2 ..., M, j=1 here, 2,…,N;

Step 1.2:The parameter of landform candidate region is calculated using mobile computing window technique, specific method is:

Define a size and be m × n local motion calculation window, and it is high to calculate the landform in local motion calculation window Spend average valueWhen the center of local mobile computing window is in each landform candidate region Total-grid point on after mobile one time, the landform standard deviation sigma (l) of each landform candidate region can be obtained, in longitudinal Landform coefficient Rlongitude(l) the landform coefficient R and on latitude directionlatitude(l), wherein l=1,2 ..., L, Subscript longitude represents longitude, and latitude represents latitude, and the parameter specific formula for calculation of landform candidate region is as follows:

Step 1.3:For the L block landform candidate region on planning flight path, each piece of landform candidate region is judged respectively Whether landform standard deviation and longitude and latitude direction landform coefficient correlation meet σ (l) simultaneously>4σcAnd Rlongitude(l)<0.7 and Rlatitude(l)<0.7, if meet, corresponding landform candidate region for can matching area, otherwise for can not matching area, its In, σcFor the standard deviation of depth sensor measurement error.

It is prior art to judge whether the method that can be matched, and calculates related to landform various for different artificially generated terrain Parameter, determines the threshold value of parameter to determine decision criteria after statistical analysis, what specific numerical value was mainly obtained by Computer Simulation.

Further, in the present invention, for landform can not matching area, the synchronous positioning and composition algorithm include with Lower step:

Step 3.1:The foundation of mission nonlinear process model

Step 3.1.1:The foundation of underwater hiding-machine state equation

It is navigational coordinate system to choose northeast day system, and carrier coordinate system x-axis points to starboard, carrier coordinate along underwater hiding-machine transverse axis It is that carrier coordinate system z-axis constitutes right-handed scale (R.H.scale) perpendicular to plane determined by x-axis and y-axis before y-axis is pointed to along the underwater hiding-machine longitudinal axis System;Then k moment underwater hiding-machines state equation is as follows:

Wherein:

posvehicle(k) position of k moment underwater hiding-machines is represented,

velvehicle(k) speed of k moment underwater hiding-machines is represented,

quavehicle(k) attitude quaternion of k moment underwater hiding-machines is represented,

Subscript G represents navigational coordinate system,

Subscript B represents carrier coordinate system,

△ t represent discrete sampling time interval,

CB2GExpression transforms to the direction cosine matrix of navigational coordinate system from carrier coordinate system,

fB(k) output of k moment accelerometer is represented,

G represents acceleration of gravity,

The quaternary number that the angular speed measured by gyroscope is constituted is represented,

Represent quaternary number multiplication;

Step 3.1.2:The foundation of map state

The setting k moment has observed NmIndividual new environmental characteristic, then k moment maps state be:

Wherein:

The state of k the m1 environmental characteristic of moment is represented,

The state of k the m2 environmental characteristic of moment is represented,

Represent k moment mNmThe state of individual environmental characteristic,

Subscript T represents transposition;

It is point feature by underwater environment feature modeling, then position of the environmental characteristic under navigational coordinate system is as follows:

Wherein:

Position of the mi environmental characteristic obtained at the k moment in navigational coordinate system is represented,

Position of the mi environmental characteristic obtained at the k-1 moment in navigational coordinate system is represented,

Represent underwater hiding-machine on orientation/range sensor to underwater hiding-machine center lever arm effect compensation rate in carrier The component of coordinate system,

Subscript S represents orientation/range sensor coordinate system, and orientation/range sensor is all integrally fixed on underwater hiding-machine, General when setting up that orientation/range sensor coordinate system is translated into a segment distance relative to carrier coordinate system x-axis, y-axis and z-axis are not Become,

CS2BThe direction cosine matrix of orientation/range sensor coordinate system transformation to carrier coordinate system is represented,

Represent sensing The relative position between mi environmental characteristic and underwater hiding-machine that device is measured, wherein, d represents sensor between environmental characteristic Distance, phi represents the azimuth between environmental characteristic and orientation/range sensor, theta represent environmental characteristic and orientation/ Elevation angle between range sensor;

Step 3.2:The foundation of mission nonlinear observation model

Step 3.2.1:Observed according to the relative position that orientation/range sensor obtains environmental characteristic and underwater hiding-machine, from And it is as follows to set up mission nonlinear observation model:

Z (k)=h (x (k))+ν (k)

Wherein:

Z (k) represents the observation to environmental characteristic,

H () is non-linear observation function,

X (k) is system state vector, including the position of underwater hiding-machine, speed, the position of attitude quaternion and environmental characteristic Put,

ν (k) is systematic observation noise;

Observation of the k moment to the mi environmental characteristic is only related to the environmental characteristic and underwater hiding-machine state, therefore has:

zmi(k)=h (xvehicle(k),xfeature_mi(k))+νmi(k)

Wherein:

zmi(k) observation of the k moment to the mi environmental characteristic is represented,

xvehicle(k) state of the underwater hiding-machine at the k moment is represented,

xfeature_mi(k) state of the mi environmental characteristic at the k moment is represented,

νmi(k) it is observation noise to the mi environmental characteristic;

Step 3.2.2:SettingThen the k moment is observed to the mi environmental characteristic:

Wherein:

D represents orientation/range sensor the distance between to environmental characteristic,

Phi represents the azimuth between environmental characteristic and orientation/range sensor,

Theta represents the elevation angle between environmental characteristic and orientation/range sensor;

Step 3.3:System mode augmentation process

During underwater hiding-machine navigation, orientation/range sensor observes environmental characteristic, if the environmental characteristic is to have observed To the feature crossed, then step 3.5 is directly performed;If the environmental characteristic is emerging feature, augmentation is carried out to system mode And the system mode that order is performed after step 3.4 and step 3.5, augmentation is changed into:

Wherein:

xvehicle(k) k moment underwater hiding-machine states, including underwater hiding-machine position, speed and attitude quaternion are represented,

xfeature(k) state of k moment environmental characteristics is represented,

fvehicle() is nonlinear state transfer function;

Step 3.4:Synchronous positioning predicts process with composition algorithm

The mission nonlinear observation model that the mission nonlinear process model and step 3.2 set up by step 3.1 are set up System mode and state covariance matrix to the k moment are predicted, and are had:

X (k | k-1)=f (x (k-1 | k-1))

Wherein:

X (k | k-1) is the one-step prediction quantity of state to the k moment according to the system mode at k-1 moment,

X (k-1 | k-1) is the system state estimation value at k-1 moment,

Pcov(k | k-1) is the prediction covariance to the k moment according to the system mode at k-1 moment,

Pcov(k-1 | k-1) is the state covariance estimate at k-1 moment,

For the Jacobian matrix of system state equation,

T represents transposition,

Q (k) is noise ω (k) covariance.

Step 3.5:According to expanded Kalman filtration algorithm to synchronous positioning and composition algorithm renewal process

The environmental characteristic obtained for observation, is updated, specifically with reference to step 3.2 and step 3.4 to system mode value Formula is as follows:

New breath:vinnov(k)=z (k)-h (x (k | k-1))

Filtering gain:

State estimation:X (k | k)=x (k | k-1)+Kgain(k)·vinnov(k)

State covariance:

Wherein:

vinnov(k) it is new breath,

Z (k) is k moment observations,

H (x (k | k-1)) predicted value is observed for the k moment,

Kgain(k) it is k moment filtering gains,

For the Jacobian matrix of systematic observation equation,

R (k) is ν (k) covariance,

X (k | k) is the system state estimation value at k moment,

Pcov(k | k) is the system mode covariance at k moment;

Step 3.6:The pos obtained from x (k | k)vehicleAs revised main inertial navigation positional information.

Above-mentioned steps 3.1-3.6 describes synchronous positioning and the main process of composition algorithm, synchronous positioning and composition algorithm Receive the initial data from inertial sensor and orientation/range sensor, it is established that the water based on inertial navigation mechanical equation Diving device state equation, and obtain by orientation/range sensor the state of environmental characteristic;Then be predicted respectively the stage, Observation stage and more new stage, forecast period is with the system state estimation value x at k-1 moment (k-1 | k-1) and covariance matrix Pcov (k-1 | k-1) based on, system mode and covariance to the k moment are predicted;The observation stage is for the environment that has existed Feature, for being updated to system mode, to new environmental characteristic, then carries out system augmentation;The more new stage, utilize observation Information between predicted value is updated to state value;Whole process is carried out using extended Kalman filter to state Prediction and renewal, without to doing linearization process to mission nonlinear process model and mission nonlinear observation model, counting Calculation amount is smaller and is easily achieved.

Beneficial effect:

The present invention is by the calculating to navigation area terrain information amount, and landform is divided into landform can matching area and landform Can not matching area.For different navigation areas, different navigation algorithms is respectively adopted to aid in main inertial navigation system, repaiies Just main inertial navigation system with time integral site error, with higher independence.

The present invention landform can matching area, main inertia is corrected using the terrain auxiliary navigation method based on ICCP algorithms The site error of navigation system is to obtain device position of more accurately diving.When underwater hiding-machine enter landform can not matching area when, can To provide synchronous positioning and the initial position of composition algorithm, to improve the precision and robustness of SLAM algorithms.

The present invention landform can not matching area, using the synchronous positioning based on marine environment feature with composition algorithm come auxiliary Main inertial navigation system is helped, and state is predicted and updated using extended Kalman filter in calculating process, And use Kalman to filter after linearization process not by being done to mission nonlinear process model and non-linearization observation model Ripple device realizes status predication and the method updated so that amount of calculation is small and more easily realizes.

Simulation result shows that the underwater hiding-machine flight path that the algorithm is obtained is more closer than the flight path that pure-inertial guidance system is obtained In Desired Track, main inertial navigation error can be overcome, which to be accumulated between, causes the problem of navigation and positioning accuracy is not high.

Brief description of the drawings

Fig. 1 is the underwater navigation alignment system schematic diagram of Combining with terrain described in the invention and environmental characteristic;

Fig. 2 is synchronous positioning and composition algorithm aided inertial navigation schematic diagram;

Fig. 3 is the topographic map employed in embodiment;

Fig. 4 is the bathymetric chart of embodiment mesorelief 1;

Fig. 5 be in embodiment underwater hiding-machine by landform can matching area when ICCP aid in the navigation of main inertial navigation system Positioning result figure;

Fig. 6 be in embodiment underwater hiding-machine by landform can not matching area when SLAM aid in main inertial navigation system to lead Navigate positioning result figure;

Embodiment

The present invention is further described below in conjunction with the accompanying drawings.

A kind of underwater navigation localization method of Combining with terrain and environmental characteristic,

The first step:According to landform information content, judge whether landform can match

Step 1.1:Sea-floor relief elevation uses the model split of grid matrix and is stored as polylith landform candidate region, water By wherein L blocks landform candidate region on the planning flight path of diving device, it is assumed that the longitude and latitude span of a certain piece of landform candidate region For M × N grids, and grid point coordinates is that the landform altitude value at (i, j) place is height (i, j), here i=1,2 ..., M, j =1,2 ..., N;

Step 1.2:The parameter of landform candidate region is calculated using mobile computing window technique, specific method is:

Define a size and be m × n local motion calculation window, and it is high to calculate the landform in local motion calculation window Spend average valueWhen the center of local mobile computing window is in each landform candidate region Total-grid point on after mobile one time, the landform standard deviation sigma (l) of each landform candidate region can be obtained, in longitudinal Landform coefficient Rlongitude(l) the landform coefficient R and on latitude directionlatitude(l), wherein l=1,2 ..., L, Subscript longitude represents longitude, and latitude represents latitude, and the parameter specific formula for calculation of landform candidate region is as follows:

Step 1.3:For the L block landform candidate region on planning flight path, each piece of landform candidate region is judged respectively Whether landform standard deviation and longitude and latitude direction landform coefficient correlation meet σ (l) simultaneously>4σcAnd Rlongitude(l)<0.7 and Rlatitude(l)<0.7, if meet, corresponding landform candidate region for can matching area, otherwise for can not matching area, its In, σcFor the standard deviation of depth sensor measurement error.

Second step:When underwater hiding-machine enter landform can matching area when, using based on isopleth closest approach iteration (ICCP) The Models in Terrain Aided Navigation of algorithm aids in correcting site error of the main inertial navigation system with time integral, specifically include with Lower step:

Step 2.1:The N of flight path measurement is provided by inertial navigation systempIndividual position value sequence, collection is combined into AndNi=1,2 ..., Np,Represent NiIndividual sequence of pointsLatitude Degree,Represent NiIndividual sequence of pointsLongitude;Real-time sounding gear provides sequence of pointsCorresponding actual measurement water Deep valueWherein Ni=1,2 ..., Np, and extract from known reference map corresponding isopleth Wherein Ni=1,2 ..., Np, and assumeClosest approach on corresponding isopleth isIt is allConstitute arrangement set

Step 2.2:Find and include spin matrixAnd translation vectorRigid transformation T, its In, θ is Random-Rotation angle, tlatitudeAnd tlongitudeIt is latitude and the translational movement of longitudinal respectively, makes arrangement setWith setThe distance between minimum, even if also following object function is minimum

Wherein:

D is object function,

The weights of sequence of points are represented,

ForWithThe distance between,

DmaxForWithThe distance between maximum;

The process for solving T is just to solve for θ, tlatitudeAnd tlongitudeProcess, can pass through Quaternion Method and construct one Hamiltonian matrix is obtained;

Step 2.3:Will setTransform to setHave Assuming that being obtained after i-th teration_k times iterationThe Obtained after iteration_k+1 iterationCalculate WithIf d (iteration_k+1)-d (iteration_k)>τ, τ value is 10-6, then return and perform step 2.2, if d (iteration_k+1)-d (iteration_k)≤τ, and iterations is less than maximum Iterations, then judge to meet final stopping criterion for iteration, exit iteration, it is [latitude to determine final matching resultsICCP longitudeICCP]T

Step 2.4:The positional information latitude that main inertial navigation system is exportedINS、longituteINSWith ICCP With obtained positional information latitudeICCP、longituteICCPDifference latitudeINS-latitudeICCPIt is used as observed quantity Kalman filter is carried out, and the site error amount obtained using filtering feeds back to main inertial navigation is exported in main inertial navigation system Position is corrected, and the position after being corrected is

3rd step:When underwater hiding-machine enter landform can not matching area when, it is fixed using the synchronization based on marine environment feature Position aids in correcting site error of the main inertial navigation system with time integral with composition (SLAM) algorithm, and its basic thought is as schemed Shown in 2, comprise the following steps:

Step 3.1:The foundation of mission nonlinear process model

Step 3.1.1:The foundation of underwater hiding-machine state equation

It is navigational coordinate system to choose northeast day system, and carrier coordinate system x-axis points to starboard, carrier coordinate along underwater hiding-machine transverse axis It is that carrier coordinate system z-axis constitutes right-handed scale (R.H.scale) perpendicular to plane determined by x-axis and y-axis before y-axis is pointed to along the underwater hiding-machine longitudinal axis System;Then k moment underwater hiding-machines state equation can be provided by main inertial navigation mechanical equation:

Wherein:

posvehicle(k) position of k moment underwater hiding-machines is represented,

velvehicle(k) speed of k moment underwater hiding-machines is represented,

quavehicle(k) attitude quaternion of k moment underwater hiding-machines is represented,

Subscript G represents navigational coordinate system,

Subscript B represents carrier coordinate system,

△ t represent discrete sampling time interval,

CB2GExpression transforms to the direction cosine matrix of navigational coordinate system from carrier coordinate system,

fB(k) output of k moment accelerometer is represented,

G represents acceleration of gravity,

The quaternary number that the angular speed measured by gyroscope is constituted is represented,

Represent quaternary number multiplication;

Step 3.1.2:The foundation of map state

The setting k moment has observed NmIndividual new environmental characteristic, then k moment maps state be:

Wherein:

The state of k the m1 environmental characteristic of moment is represented,

The state of k the m2 environmental characteristic of moment is represented,

Represent k moment mNmThe state of individual environmental characteristic,

Subscript T represents transposition;

It is point feature by underwater environment feature modeling, then position of the environmental characteristic under navigational coordinate system is as follows:

Wherein:

Position of the mi environmental characteristic obtained at the k moment in navigational coordinate system is represented,

Position of the mi environmental characteristic obtained at the k-1 moment in navigational coordinate system is represented,

Represent underwater hiding-machine on orientation/range sensor to underwater hiding-machine center lever arm effect compensation rate in carrier The component of coordinate system,

Subscript S represents orientation/range sensor coordinate system,

CS2BThe direction cosine matrix of orientation/range sensor coordinate system transformation to carrier coordinate system is represented,

Expression orientation/ The the relative position between mi environmental characteristic and underwater hiding-machine that range sensor is measured, wherein, d represents that orientation/distance is passed Sensor is the distance between to environmental characteristic, and phi and theta represent the side between environmental characteristic and orientation/range sensor respectively Parallactic angle and elevation angle;

Step 3.2:The foundation of mission nonlinear observation model

Step 3.2.1:Orientation/range sensor on underwater hiding-machine can obtain the relative of environmental characteristic and underwater hiding-machine Position detection, mission nonlinear observation model can be expressed as follows:

Z (k)=h (x (k))+ν (k)

Wherein:

Z (k) represents the observation to environmental characteristic,

H () is non-linear observation function,

X (k) is system state vector, including the position of underwater hiding-machine, speed, the position of attitude quaternion and environmental characteristic Put,

ν (k) is systematic observation noise;

Observation of the k moment to the mi environmental characteristic is only related to this feature and underwater hiding-machine state, therefore has:

zmi(k)=h (xvehicle(k),xfeature_mi(k))+νmi(k)

Wherein:

zmi(k) observation of the k moment to the mi environmental characteristic is represented,

xvehicle(k) state of the underwater hiding-machine at the k moment is represented,

xfeature_mi(k) state of the mi environmental characteristic at the k moment is represented,

νmi(k) it is observation noise to the mi environmental characteristic;

Step 3.2.2:SettingThen the k moment is observed to the mi environmental characteristic:

Wherein:

D represents orientation/range sensor the distance between to environmental characteristic,

Phi represents the azimuth between environmental characteristic and orientation/range sensor,

Theta represents the elevation angle between environmental characteristic and orientation/range sensor;

Step 3.3:System mode augmentation process

During underwater hiding-machine navigation, orientation/range sensor observes environmental characteristic, if the environmental characteristic is to have observed To the feature crossed, then step 3.5 is directly performed;If the environmental characteristic is emerging feature, augmentation is carried out to system mode And the system mode that order is performed after step 3.4 and step 3.5, augmentation is changed into:

Wherein:

xvehicle(k) k moment underwater hiding-machine states, including underwater hiding-machine position, speed and posture are represented,

xfeature(k) state of environmental characteristic is represented, because the position that the state of environmental characteristic only includes environmental characteristic is believed Breath, Gu xfeature(k) the actual position for representing environmental characteristic,

fvehicle() is nonlinear state transfer function;

Step 3.4:SLAM algorithms predict process

The system mode and state covariance matrix at k moment are predicted by process model and observation model, had:

X (k | k-1)=f (x (k-1 | k-1))

Wherein:

X (k | k-1) is the one-step prediction quantity of state to the k moment according to the system mode at k-1 moment,

X (k-1 | k-1) is the system state estimation value at k-1 moment,

Pcov(k | k-1) is the prediction covariance to the k moment according to the system mode at k-1 moment,

Pcov(k-1 | k-1) is the state covariance estimate at k-1 moment,

For the Jacobian matrix of system state equation,

T represents transposition,

Q (k) is noise ω (k) covariance;

Step 3.5:SLAM algorithm renewal processes

The environmental characteristic obtained for observation, is updated, specifically with reference to step 3.2 and step 3.4 to system mode value Formula is as follows:

New breath:vinnov(k)=z (k)-h (x (k | k-1))

Filtering gain:

State estimation:X (k | k)=x (k | k-1)+Kgain(k)·vinnov(k)

State covariance:

Wherein:

vinnov(k) it is new breath,

Z (k) is k moment observations,

H (x (k | k-1)) predicted value is observed for the k moment,

Kgain(k) it is k moment filtering gains,

For the Jacobian matrix of systematic observation equation,

R (k) is ν (k) covariance,

X (k | k) is the system state estimation value at k moment,

Pcov(k | k) is the system mode covariance at k moment;

Step 3.6:The pos obtained in SLAM algorithms from x (k | k)vehicleAs revised main inertial navigation position Information.

Embodiment:

Emulation experiment chooses shaped area as shown in Figure 3, in the range of (38.0 ° of north latitude, 120.0 ° of east longitude) to (north latitude 38.04 °, 120.05 ° of east longitude) rectangular area, according to mobile computing window technique, obtained by simulation analysis such as the left side in Fig. 4 Blocked areas be terrain match region, the blocked areas on the right mismatches region for landform.

The 1# lines in landform can set a preferable guidance path containing 10 points, such as Fig. 5 in matching area, inertial navigation refers to Show 2# lines in flight path such as Fig. 5, the matching flight path after matching positioning is carried out as shown in 3# lines in Fig. 5 using ICCP algorithms, can be seen Arrive, matching flight path is almost overlapped with Desired Track.

Landform can not matching area, using SLAM algorithms carry out navigator fix.As in Fig. 6,3 are arbitrarily provided in the environment Individual environmental characteristic simultaneously sets one to be led using ICCP aided inertial navigations system gained final position as the ideal of 10 points of starting point Bit path, as shown in 1# lines in Fig. 6.Inertial navigation indicates flight path as shown in 2# lines in Fig. 6, the estimated path obtained using SLAM algorithms As shown in 3# lines in Fig. 6.It can be seen that SLAM estimates that flight path indicates flight path closer to Desired Track than pure-inertial guidance system.

From the point of view of simulation result, after algorithm may determine that whether landform can match, suitable assisting navigation is independently selected Means, landform can matching area, using the Models in Terrain Aided Navigation based on ICCP algorithms, carry out aided inertial navigation system; Landform can not matching area, using SLAM algorithms come aided inertial navigation system, so as to realize that submarine navigation device is prolonged Independent navigation.

Described above is only the preferred embodiment of the present invention, it should be pointed out that:For the ordinary skill people of the art For member, under the premise without departing from the principles of the invention, some improvements and modifications can also be made, these improvements and modifications are also regarded For protection scope of the present invention.

Claims (2)

1. the underwater navigation localization method of a kind of Combining with terrain and environmental characteristic, it is characterised in that:Judged according to landform information content Whether each sub-regions can match in the planning flight path of underwater hiding-machine, real using terrain aided inertial navigation system if it can match Now position, aid in main inertial navigation system to realize using synchronous positioning and composition algorithm if it can not match and position;
Described synchronous positioning comprises the following steps with composition algorithm:
Step 3.1:The foundation of mission nonlinear process model
Step 3.1.1:The foundation of underwater hiding-machine state equation
It is navigational coordinate system to choose northeast day system, and carrier coordinate system x-axis points to starboard, carrier coordinate system y along underwater hiding-machine transverse axis Before axle is pointed to along the underwater hiding-machine longitudinal axis, carrier coordinate system z-axis constitutes right-handed coordinate system perpendicular to plane determined by x-axis and y-axis; Then k moment underwater hiding-machines state equation is as follows:
Wherein:
posvehicle(k) position of k moment underwater hiding-machines is represented,
velvehicle(k) speed of k moment underwater hiding-machines is represented,
quavehicle(k) attitude quaternion of k moment underwater hiding-machines is represented,
Subscript G represents navigational coordinate system,
Subscript B represents carrier coordinate system,
△ t represent discrete sampling time interval,
CB2GExpression transforms to the direction cosine matrix of navigational coordinate system from carrier coordinate system,
fB(k) output of k moment accelerometer is represented,
G represents acceleration of gravity,
The quaternary number that the angular speed measured by gyroscope is constituted is represented,
Represent quaternary number multiplication;
Step 3.1.2:The foundation of map state
The setting k moment has observed NmIndividual new environmental characteristic, then k moment maps state be:
<mrow> <msub> <mi>x</mi> <mrow> <mi>f</mi> <mi>e</mi> <mi>a</mi> <mi>t</mi> <mi>u</mi> <mi>r</mi> <mi>e</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>x</mi> <mrow> <mi>f</mi> <mi>e</mi> <mi>a</mi> <mi>t</mi> <mi>u</mi> <mi>r</mi> <mi>e</mi> <mo>_</mo> <mi>m</mi> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>x</mi> <mrow> <mi>f</mi> <mi>e</mi> <mi>a</mi> <mi>t</mi> <mi>u</mi> <mi>r</mi> <mi>e</mi> <mo>_</mo> <mi>m</mi> <mn>2</mn> </mrow> <mi>T</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mrow> <msubsup> <mi>x</mi> <mrow> <mi>f</mi> <mi>e</mi> <mi>a</mi> <mi>t</mi> <mi>u</mi> <mi>r</mi> <mi>e</mi> <mo>_</mo> <msub> <mi>mN</mi> <mi>m</mi> </msub> </mrow> <mi>T</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow>
Wherein:
The state of k the m1 environmental characteristic of moment is represented,
The state of k the m2 environmental characteristic of moment is represented,
Represent k moment mNmThe state of individual environmental characteristic,
Subscript T represents transposition;
It is point feature by underwater environment feature modeling, then position of the environmental characteristic under navigational coordinate system is as follows:
<mrow> <msubsup> <mi>x</mi> <mrow> <mi>f</mi> <mi>e</mi> <mi>a</mi> <mi>t</mi> <mi>u</mi> <mi>r</mi> <mi>e</mi> <mo>_</mo> <mi>m</mi> <mi>i</mi> </mrow> <mi>G</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>x</mi> <mrow> <mi>f</mi> <mi>e</mi> <mi>a</mi> <mi>t</mi> <mi>u</mi> <mi>r</mi> <mi>e</mi> <mo>_</mo> <mi>m</mi> <mi>i</mi> </mrow> <mi>G</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>pos</mi> <mrow> <mi>v</mi> <mi>e</mi> <mi>h</mi> <mi>i</mi> <mi>c</mi> <mi>l</mi> <mi>e</mi> </mrow> <mi>G</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>C</mi> <mrow> <mi>B</mi> <mn>2</mn> <mi>G</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msubsup> <mi>r</mi> <mrow> <mi>B</mi> <mi>S</mi> </mrow> <mi>B</mi> </msubsup> <mo>+</mo> <msub> <mi>C</mi> <mrow> <mi>B</mi> <mn>2</mn> <mi>G</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msub> <mi>C</mi> <mrow> <mi>S</mi> <mn>2</mn> <mi>B</mi> </mrow> </msub> <msubsup> <mi>r</mi> <mrow> <mi>S</mi> <mi>f</mi> <mi>e</mi> <mi>a</mi> <mi>t</mi> <mi>u</mi> <mi>r</mi> <mi>e</mi> <mo>_</mo> <mi>m</mi> <mi>i</mi> </mrow> <mi>S</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow>
Wherein:
Position of the mi environmental characteristic obtained at the k moment in navigational coordinate system is represented,
Position of the mi environmental characteristic obtained at the k-1 moment in navigational coordinate system is represented,
Represent underwater hiding-machine on orientation/range sensor to underwater hiding-machine center lever arm effect compensation rate in carrier coordinate The component of system,
Subscript S represents orientation/range sensor coordinate system,
CS2BThe direction cosine matrix of orientation/range sensor coordinate system transformation to carrier coordinate system is represented,
Represent orientation/distance The relative position between mi environmental characteristic and underwater hiding-machine that sensor is measured, wherein, d represents orientation/range sensor The distance between environmental characteristic is arrived, phi represents the azimuth between environmental characteristic and orientation/range sensor, and theta represents ring Elevation angle between border feature and orientation/range sensor;
Step 3.2:The foundation of mission nonlinear observation model
Step 3.2.1:Observed according to the relative position that orientation/range sensor obtains environmental characteristic and underwater hiding-machine, so as to build The non-linear observation model of erection system is as follows:
Z (k)=h (x (k))+ν (k)
Wherein:
Z (k) represents the observation to environmental characteristic,
H () is non-linear observation function,
X (k) is system state vector, including the position of underwater hiding-machine, speed, the position of attitude quaternion and environmental characteristic,
ν (k) is systematic observation noise;
Observation of the k moment to the mi environmental characteristic is only related to the environmental characteristic and underwater hiding-machine state, therefore has:
zmi(k)=h (xvehicle(k),xfeature_mi(k))+νmi(k)
Wherein:
zmi(k) observation of the k moment to the mi environmental characteristic is represented,
xvehicle(k) state of the underwater hiding-machine at the k moment is represented,
xfeature_mi(k) state of the mi environmental characteristic at the k moment is represented,
νmi(k) it is observation noise to the mi environmental characteristic;
Step 3.2.2:SettingThen the k moment is observed to the mi environmental characteristic:
<mrow> <msub> <mi>z</mi> <mrow> <mi>m</mi> <mi>i</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mi>d</mi> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>p</mi> <mi>h</mi> <mi>i</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>t</mi> <mi>h</mi> <mi>e</mi> <mi>t</mi> <mi>a</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msqrt> <mrow> <msup> <mi>x</mi> <mn>2</mn> </msup> <mo>+</mo> <msup> <mi>y</mi> <mn>2</mn> </msup> <mo>+</mo> <msup> <mi>z</mi> <mn>2</mn> </msup> </mrow> </msqrt> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>a</mi> <mi>r</mi> <mi>c</mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mi>y</mi> <mi>x</mi> </mfrac> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>arctan</mi> <mrow> <mo>(</mo> <mfrac> <mi>z</mi> <msqrt> <mrow> <msup> <mi>x</mi> <mn>2</mn> </msup> <mo>+</mo> <msup> <mi>y</mi> <mn>2</mn> </msup> </mrow> </msqrt> </mfrac> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
Wherein:
D represents orientation/range sensor the distance between to environmental characteristic,
Phi represents the azimuth between environmental characteristic and orientation/range sensor,
Theta represents the elevation angle between environmental characteristic and orientation/range sensor;
Step 3.3:System mode augmentation process
During underwater hiding-machine navigation, orientation/range sensor observes environmental characteristic, if the environmental characteristic is to have been observed that Feature, then directly perform step 3.5;If the environmental characteristic is emerging feature, augmentation is carried out to system mode and suitable The system mode that sequence performs after step 3.4 and step 3.5, augmentation is changed into:
<mrow> <mi>x</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mrow> <mi>v</mi> <mi>e</mi> <mi>h</mi> <mi>i</mi> <mi>c</mi> <mi>l</mi> <mi>e</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mrow> <mi>f</mi> <mi>e</mi> <mi>a</mi> <mi>t</mi> <mi>u</mi> <mi>r</mi> <mi>e</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>f</mi> <mrow> <mi>v</mi> <mi>e</mi> <mi>h</mi> <mi>i</mi> <mi>c</mi> <mi>l</mi> <mi>e</mi> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>v</mi> <mi>e</mi> <mi>h</mi> <mi>i</mi> <mi>c</mi> <mi>l</mi> <mi>e</mi> </mrow> </msub> <mo>(</mo> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>)</mo> <mo>,</mo> <mi>u</mi> <mo>(</mo> <mi>k</mi> <mo>)</mo> <mo>,</mo> <mi>w</mi> <mo>(</mo> <mi>k</mi> <mo>)</mo> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mrow> <mi>f</mi> <mi>e</mi> <mi>a</mi> <mi>t</mi> <mi>u</mi> <mi>r</mi> <mi>e</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
Wherein:
xvehicle(k) k moment underwater hiding-machine states, including underwater hiding-machine position, speed and attitude quaternion are represented,
xfeature(k) state of k moment environmental characteristics is represented,
fvehicle() is nonlinear state transfer function;
Step 3.4:Synchronous positioning predicts process with composition algorithm
When the mission nonlinear observation model that the mission nonlinear process model and step 3.2 set up by step 3.1 are set up is to k The system mode and state covariance matrix at quarter are predicted, and are had:
X (k | k-1)=f (x (k-1 | k-1))
<mrow> <msub> <mi>P</mi> <mi>cov</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>=</mo> <mover> <mi>F</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msub> <mi>P</mi> <mi>cov</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mover> <mi>F</mi> <mo>&amp;OverBar;</mo> </mover> <msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <mi>Q</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow>
Wherein:
X (k | k-1) is the one-step prediction quantity of state to the k moment according to the system mode at k-1 moment,
X (k-1 | k-1) is the system state estimation value at k-1 moment,
Pcov(k | k-1) is the prediction covariance to the k moment according to the system mode at k-1 moment,
Pcov(k-1 | k-1) is the state covariance estimate at k-1 moment,
For the Jacobian matrix of system state equation,
T represents transposition,
Q (k) is noise ω (k) covariance;
Step 3.5:Synchronous positioning and composition algorithm renewal process
The environmental characteristic obtained for observation, is updated, specific formula with reference to step 3.2 and step 3.4 to system mode value It is as follows:
New breath:vinnov(k)=z (k)-h (x (k | k-1))
Filtering gain:
State estimation:X (k | k)=x (k | k-1)+Kgain(k)·vinnov(k)
State covariance:
Wherein:
vinnov(k) it is new breath,
Z (k) is k moment observations,
H (x (k | k-1)) predicted value is observed for the k moment,
Kgain(k) it is k moment filtering gains,
For the Jacobian matrix of systematic observation equation,
R (k) is ν (k) covariance,
X (k | k) is the system state estimation value at k moment,
Pcov(k | k) is the system mode covariance at k moment;
Step 3.6:The pos obtained from x (k | k)vehicleAs revised main inertial navigation positional information.
2. the underwater navigation localization method of Combining with terrain according to claim 1 and environmental characteristic, it is characterised in that:Judge The method whether each sub-regions in the planning flight path of underwater hiding-machine can match is as follows:
Step 1.1:By sea-floor relief elevation using the model split of grid matrix into polylith landform candidate region, underwater hiding-machine Plan on flight path by wherein L blocks landform candidate region, set the longitude and latitude span of a certain piece of landform candidate region as M × N nets Lattice, and grid point coordinates is that the landform altitude value at (i, j) place is height (i, j), i=1,2 ..., M, j=1 here, 2,…,N;
Step 1.2:The parameter of landform candidate region is calculated using mobile computing window technique, specific method is:
Define a size and be m × n local motion calculation window, and calculate the Terrain Elevation in local motion calculation window and put down AverageWhen the center of local mobile computing window is in each landform candidate region On total-grid point after mobile one time, the landform standard deviation sigma (l) of each landform candidate region, the ground in longitudinal can be obtained Shape coefficient Rlongitude(l) the landform coefficient R and on latitude directionlatitude(l), wherein l=1,2 ..., L, under Mark longitude represents longitude, and latitude represents latitude, and the parameter specific formula for calculation of landform candidate region is as follows:
<mrow> <mi>&amp;sigma;</mi> <mrow> <mo>(</mo> <mi>l</mi> <mo>)</mo> </mrow> <mo>=</mo> <msqrt> <mrow> <mfrac> <mn>1</mn> <mrow> <mi>m</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>m</mi> </munderover> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msup> <mrow> <mo>&amp;lsqb;</mo> <mi>h</mi> <mi>e</mi> <mi>i</mi> <mi>g</mi> <mi>h</mi> <mi>t</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>-</mo> <mi>h</mi> <mi>e</mi> <mi>i</mi> <mi>g</mi> <mi>h</mi> <mi>t</mi> <mo>_</mo> <mi>a</mi> <mi>v</mi> <mi>e</mi> <mo>&amp;rsqb;</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> </mrow>
<mrow> <msub> <mi>R</mi> <mrow> <mi>l</mi> <mi>o</mi> <mi>n</mi> <mi>g</mi> <mi>i</mi> <mi>t</mi> <mi>u</mi> <mi>d</mi> <mi>e</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>l</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mi>n</mi> <mrow> <mo>(</mo> <mi>m</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mi>&amp;sigma;</mi> <msup> <mrow> <mo>(</mo> <mi>l</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mi>m</mi> <mo>-</mo> <mn>1</mn> </mrow> </munderover> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mo>&amp;lsqb;</mo> <mi>h</mi> <mi>e</mi> <mi>i</mi> <mi>g</mi> <mi>h</mi> <mi>t</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>-</mo> <mi>h</mi> <mi>e</mi> <mi>i</mi> <mi>g</mi> <mi>h</mi> <mi>t</mi> <mo>_</mo> <mi>a</mi> <mi>v</mi> <mi>e</mi> <mo>&amp;rsqb;</mo> <mo>&amp;lsqb;</mo> <mi>h</mi> <mi>e</mi> <mi>i</mi> <mi>g</mi> <mi>h</mi> <mi>t</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>-</mo> <mi>h</mi> <mi>e</mi> <mi>i</mi> <mi>g</mi> <mi>h</mi> <mi>t</mi> <mo>_</mo> <mi>a</mi> <mi>v</mi> <mi>e</mi> <mo>&amp;rsqb;</mo> </mrow>
<mrow> <msub> <mi>R</mi> <mrow> <mi>l</mi> <mi>a</mi> <mi>t</mi> <mi>i</mi> <mi>t</mi> <mi>u</mi> <mi>d</mi> <mi>e</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>l</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mi>n</mi> <mrow> <mo>(</mo> <mi>m</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mi>&amp;sigma;</mi> <msup> <mrow> <mo>(</mo> <mi>l</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>m</mi> </munderover> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mi>n</mi> <mo>-</mo> <mn>1</mn> </mrow> </munderover> <mo>&amp;lsqb;</mo> <mi>h</mi> <mi>e</mi> <mi>i</mi> <mi>g</mi> <mi>h</mi> <mi>t</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>-</mo> <mi>h</mi> <mi>e</mi> <mi>i</mi> <mi>g</mi> <mi>h</mi> <mi>t</mi> <mo>_</mo> <mi>a</mi> <mi>v</mi> <mi>e</mi> <mo>&amp;rsqb;</mo> <mo>&amp;lsqb;</mo> <mi>h</mi> <mi>e</mi> <mi>i</mi> <mi>g</mi> <mi>h</mi> <mi>t</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>+</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>-</mo> <mi>h</mi> <mi>e</mi> <mi>i</mi> <mi>g</mi> <mi>h</mi> <mi>t</mi> <mo>_</mo> <mi>a</mi> <mi>v</mi> <mi>e</mi> <mo>&amp;rsqb;</mo> </mrow>
Step 1.3:For the L block landform candidate region on planning flight path, the landform of each piece of landform candidate region is judged respectively Whether standard deviation and longitude and latitude direction landform coefficient correlation meet σ (l) simultaneously>4σcAnd Rlongitude(l)<0.7 and Rlatitude(l) <0.7, if meet, corresponding landform candidate region for can matching area, otherwise for can not matching area, wherein, σcFor depth measurement The standard deviation of sensor measurement errors.
CN201410320791.6A 2014-07-07 2014-07-07 A kind of underwater navigation localization method of Combining with terrain and environmental characteristic CN104075715B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201410320791.6A CN104075715B (en) 2014-07-07 2014-07-07 A kind of underwater navigation localization method of Combining with terrain and environmental characteristic

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201410320791.6A CN104075715B (en) 2014-07-07 2014-07-07 A kind of underwater navigation localization method of Combining with terrain and environmental characteristic

Publications (2)

Publication Number Publication Date
CN104075715A CN104075715A (en) 2014-10-01
CN104075715B true CN104075715B (en) 2017-09-01

Family

ID=51597151

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201410320791.6A CN104075715B (en) 2014-07-07 2014-07-07 A kind of underwater navigation localization method of Combining with terrain and environmental characteristic

Country Status (1)

Country Link
CN (1) CN104075715B (en)

Families Citing this family (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105333869A (en) * 2015-11-04 2016-02-17 天津津航计算技术研究所 Unmanned reconnaissance aerial vehicle synchronous positioning and picture compositing method based on self-adaption EKF
CN105424036B (en) * 2015-11-09 2018-02-13 东南大学 A kind of inexpensive underwater hiding-machine terrain aided inertia combined navigation localization method
CN105547300A (en) * 2015-12-30 2016-05-04 航天恒星科技有限公司 All-source navigation system and method used for AUV (Autonomous Underwater Vehicle)
CN106052688B (en) * 2016-08-08 2019-01-08 西安电子科技大学 Inertial navigation system speed accumulation error correcting method based on terrain contour matching
CN107167126B (en) * 2017-03-31 2019-09-20 大鹏高科(武汉)智能装备有限公司 A kind of autonomous type underwater robot Combinated navigation method and system
CN107132521B (en) * 2017-05-16 2019-12-06 哈尔滨工程大学 method for judging correctness of terrain matching result in BSLAM (binary-coded decimal motion)
CN107727096A (en) * 2017-09-15 2018-02-23 哈尔滨工程大学 AUV terrain match localization methods based on the screening of effective node
CN108344999B (en) * 2018-01-09 2020-08-11 浙江大学 Sonar map construction and repositioning method for underwater robot navigation
CN108318034B (en) * 2018-01-09 2020-05-22 浙江大学 AUV docking navigation method based on sonar map
CN108362281A (en) * 2018-02-24 2018-08-03 中国人民解放军61540部队 A kind of Long baselines underwater submarine matching navigation method and system
CN108592916B (en) * 2018-04-20 2020-08-07 杭州电子科技大学 Multi-navigation map positioning and navigation method of suspended autonomous underwater vehicle
RU2709100C1 (en) * 2018-06-19 2019-12-16 Федеральное государственное бюджетное учреждение науки Специальное конструкторское бюро средств автоматизации морских исследований Дальневосточного отделения Российской академии наук Method of determining location of underwater object
CN109443343A (en) * 2018-09-13 2019-03-08 安徽优思天成智能科技有限公司 A kind of Target Tracking System
CN109186610A (en) * 2018-10-15 2019-01-11 哈尔滨工程大学 A kind of robust BSLAM method of AUV terrain match navigation

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101788295A (en) * 2010-02-26 2010-07-28 南京信息工程大学 Combined navigation system of small-scale underwater vehicle and method thereof
CN103542851B (en) * 2013-11-04 2016-03-23 东南大学 A kind of submarine navigation device assisting navigation localization method based on underwater topography elevation database

Also Published As

Publication number Publication date
CN104075715A (en) 2014-10-01

Similar Documents

Publication Publication Date Title
Allotta et al. A new AUV navigation system exploiting unscented Kalman filter
Wu et al. Velocity/position integration formula part I: Application to in-flight coarse alignment
Georgy et al. Low-cost three-dimensional navigation solution for RISS/GPS integration using mixture particle filter
Webster et al. Advances in single-beacon one-way-travel-time acoustic navigation for underwater vehicles
CN103314274B (en) The evaluation method of the track of moving element or object and system
Kinsey et al. A survey of underwater vehicle navigation: Recent advances and new challenges
CN104197927B (en) Submerged structure detects robot real-time navigation system and method
CN104316045B (en) A kind of AUV based on SINS/LBL interacts aided positioning system and localization method under water
Eustice et al. Experimental results in synchronous-clock one-way-travel-time acoustic navigation for autonomous underwater vehicles
CN101788296B (en) SINS/CNS deep integrated navigation system and realization method thereof
US7868821B2 (en) Method and apparatus to estimate vehicle position and recognized landmark positions using GPS and camera
CN102829785B (en) Air vehicle full-parameter navigation method based on sequence image and reference image matching
Jiancheng et al. Study on innovation adaptive EKF for in-flight alignment of airborne POS
Hong et al. A car test for the estimation of GPS/INS alignment errors
CN106017463A (en) Aircraft positioning method based on positioning and sensing device
CN104635251B (en) A kind of INS/GPS integrated positionings determine appearance new method
CN101354253B (en) Geomagnetic auxiliary navigation algorithm based on matching degree
Georgy et al. Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation
CN104215259B (en) A kind of ins error bearing calibration based on earth magnetism modulus gradient and particle filter
US6819984B1 (en) LOST 2—a positioning system for under water vessels
CN103776446B (en) A kind of pedestrian&#39;s independent navigation computation based on double MEMS-IMU
Gebre-Egziabher et al. MAV attitude determination by vector matching
CN104181574B (en) A kind of SINS/GLONASS integrated navigation filtering system and method
Meduna et al. Closed-loop terrain relative navigation for AUVs with non-inertial grade navigation sensors
KR20070059105A (en) Improved gps accumulated delta range processing for navigation processing

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant