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 PDFInfo
 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
Links
 230000004807 localization Effects 0.000 title claims abstract description 10
 238000004422 calculation algorithm Methods 0.000 claims abstract description 22
 230000001360 synchronised Effects 0.000 claims abstract description 16
 238000000034 methods Methods 0.000 claims description 29
 239000000969 carriers Substances 0.000 claims description 23
 239000011159 matrix materials Substances 0.000 claims description 21
 238000004364 calculation methods Methods 0.000 claims description 11
 230000003416 augmentation Effects 0.000 claims description 10
 238000001914 filtration Methods 0.000 claims description 8
 230000000875 corresponding Effects 0.000 claims description 6
 230000017105 transposition Effects 0.000 claims description 6
 230000000694 effects Effects 0.000 claims description 4
 230000001131 transforming Effects 0.000 claims description 4
 230000001133 acceleration Effects 0.000 claims description 3
 230000005484 gravity Effects 0.000 claims description 3
 238000005070 sampling Methods 0.000 claims description 3
 239000010950 nickel Substances 0.000 description 5
 239000011901 water Substances 0.000 description 5
 238000004088 simulation Methods 0.000 description 3
 241001643900 Diaspidiotus ancylus Species 0.000 description 2
 241000197722 Sphaeroceridae Species 0.000 description 2
 238000010586 diagrams Methods 0.000 description 2
 238000005516 engineering processes Methods 0.000 description 2
 230000004048 modification Effects 0.000 description 2
 238000006011 modification reactions Methods 0.000 description 2
 238000009825 accumulation Methods 0.000 description 1
 238000004458 analytical methods Methods 0.000 description 1
 235000020127 ayran Nutrition 0.000 description 1
 238000005094 computer simulation Methods 0.000 description 1
 238000002715 modification method Methods 0.000 description 1
 230000002035 prolonged Effects 0.000 description 1
 238000007619 statistical methods Methods 0.000 description 1
Classifications

 G—PHYSICS
 G01—MEASURING; TESTING
 G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
 G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00  G01C19/00
 G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00  G01C19/00 by using measurements of speed or acceleration
 G01C21/12—Navigation; 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/16—Navigation; 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/165—Navigation; 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 noninertial navigation instruments

 G—PHYSICS
 G01—MEASURING; TESTING
 G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
 G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00  G01C19/00
 G01C21/20—Instruments for performing navigational calculations
Abstract
Description
Technical field
The present invention relates to underwater navigation technical field, specifically design one kind and disclosure satisfy that underwater hidingmachine 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 threedimensional 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 hidingmachine 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 (TerrainAided Navigation, TAN), is substantially by inertial navigation system (providing realtime figure) is constituted with sensor (realtime figure is contacted and tie with reference map) and numerical map (providing reference map) Integrated navigation system, it is used as a kind of highprecision 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.Terrainaided Navigation has There is autonomous, hidden, continuous, all weather operations, navigation positioning error, not with the advantage of time integral, is that underwater hidingmachine 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, Terrainaided 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 Terrainaided Navigation, to correct the position of underwater hidingmachine 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 nonavailability 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 hidingmachine is judged according to landform information content Whether each subregions 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 hidingmachine；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 hidingmachine has more preferable independence and accuracy.
Further, in the present invention, judge what whether each subregions in the planning flight path of underwater hidingmachine can match Method is as follows：
Step 1.1：Seafloor 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 Totalgrid point on after mobile one time, the landform standard deviation sigma (l) of each landform candidate region can be obtained, in longitudinal Landform coefficient R_{longitude}(l) the landform coefficient R and on latitude direction_{latitude}(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σ_{c}And R_{longitude}(l)<0.7 and R_{latitude}(l)<0.7, if meet, corresponding landform candidate region for can matching area, otherwise for can not matching area, its In, σ_{c}For 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 hidingmachine state equation
It is navigational coordinate system to choose northeast day system, and carrier coordinate system xaxis points to starboard, carrier coordinate along underwater hidingmachine transverse axis It is that carrier coordinate system zaxis constitutes righthanded scale (R.H.scale) perpendicular to plane determined by xaxis and yaxis before yaxis is pointed to along the underwater hidingmachine longitudinal axis System；Then k moment underwater hidingmachines state equation is as follows：
Wherein：
pos_{vehicle}(k) position of k moment underwater hidingmachines is represented,
vel_{vehicle}(k) speed of k moment underwater hidingmachines is represented,
qua_{vehicle}(k) attitude quaternion of k moment underwater hidingmachines is represented,
Subscript G represents navigational coordinate system,
Subscript B represents carrier coordinate system,
△ t represent discrete sampling time interval,
C_{B2G}Expression transforms to the direction cosine matrix of navigational coordinate system from carrier coordinate system,
f^{B}(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 N_{m}Individual 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 mN_{m}The 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 k1 moment in navigational coordinate system is represented,
Represent underwater hidingmachine on orientation/range sensor to underwater hidingmachine 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 hidingmachine, General when setting up that orientation/range sensor coordinate system is translated into a segment distance relative to carrier coordinate system xaxis, yaxis and zaxis are not Become,
C_{S2B}The 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 hidingmachine 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 hidingmachine, 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 nonlinear observation function,
X (k) is system state vector, including the position of underwater hidingmachine, 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 hidingmachine state, therefore has：
z_{mi}(k)=h (x_{vehicle}(k),x_{feature_mi}(k))+ν_{mi}(k)
Wherein：
z_{mi}(k) observation of the k moment to the mi environmental characteristic is represented,
x_{vehicle}(k) state of the underwater hidingmachine at the k moment is represented,
x_{feature_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 hidingmachine 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：
x_{vehicle}(k) k moment underwater hidingmachine states, including underwater hidingmachine position, speed and attitude quaternion are represented,
x_{feature}(k) state of k moment environmental characteristics is represented,
f_{vehicle}() 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  k1)=f (x (k1  k1))
Wherein：
X (k  k1) is the onestep prediction quantity of state to the k moment according to the system mode at k1 moment,
X (k1  k1) is the system state estimation value at k1 moment,
P_{cov}(k  k1) is the prediction covariance to the k moment according to the system mode at k1 moment,
P_{cov}(k1  k1) is the state covariance estimate at k1 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：v_{innov}(k)=z (k)h (x (k  k1))
Filtering gain：
State estimation：X (k  k)=x (k  k1)+K_{gain}(k)·v_{innov}(k)
State covariance：
Wherein：
v_{innov}(k) it is new breath,
Z (k) is k moment observations,
H (x (k  k1)) predicted value is observed for the k moment,
K_{gain}(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,
P_{cov}(k  k) is the system mode covariance at k moment；
Step 3.6：The pos obtained from x (k  k)_{vehicle}As revised main inertial navigation positional information.
Abovementioned steps 3.13.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 k1 moment (k1  k1) and covariance matrix P_{cov} (k1  k1) 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 hidingmachine 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 nonlinearization 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 hidingmachine flight path that the algorithm is obtained is more closer than the flight path that pureinertial 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 hidingmachine 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 hidingmachine 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：Seafloor 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 Totalgrid point on after mobile one time, the landform standard deviation sigma (l) of each landform candidate region can be obtained, in longitudinal Landform coefficient R_{longitude}(l) the landform coefficient R and on latitude direction_{latitude}(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σ_{c}And R_{longitude}(l)<0.7 and R_{latitude}(l)<0.7, if meet, corresponding landform candidate region for can matching area, otherwise for can not matching area, its In, σ_{c}For the standard deviation of depth sensor measurement error.
Second step：When underwater hidingmachine 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 system_{p}Individual position value sequence, collection is combined into AndN_{i}=1,2 ..., N_{p},Represent N_{i}Individual sequence of pointsLatitude Degree,Represent N_{i}Individual sequence of pointsLongitude；Realtime sounding gear provides sequence of pointsCorresponding actual measurement water Deep valueWherein N_{i}=1,2 ..., N_{p}, and extract from known reference map corresponding isopleth Wherein N_{i}=1,2 ..., N_{p}, 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 RandomRotation angle, t_{latitude}And t_{longitude}It 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,
D_{max}ForWithThe distance between maximum；
The process for solving T is just to solve for θ, t_{latitude}And t_{longitude}Process, can pass through Quaternion Method and construct one Hamiltonian matrix is obtained；
Step 2.3：Will setTransform to setHave Assuming that being obtained after ith 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 results_{ICCP} longitude_{ICCP}]^{T}；
Step 2.4：The positional information latitude that main inertial navigation system is exported_{INS}、longitute_{INS}With ICCP With obtained positional information latitude_{ICCP}、longitute_{ICCP}Difference latitude_{INS}latitude_{ICCP}It 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 hidingmachine 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 hidingmachine state equation
It is navigational coordinate system to choose northeast day system, and carrier coordinate system xaxis points to starboard, carrier coordinate along underwater hidingmachine transverse axis It is that carrier coordinate system zaxis constitutes righthanded scale (R.H.scale) perpendicular to plane determined by xaxis and yaxis before yaxis is pointed to along the underwater hidingmachine longitudinal axis System；Then k moment underwater hidingmachines state equation can be provided by main inertial navigation mechanical equation：
Wherein：
pos_{vehicle}(k) position of k moment underwater hidingmachines is represented,
vel_{vehicle}(k) speed of k moment underwater hidingmachines is represented,
qua_{vehicle}(k) attitude quaternion of k moment underwater hidingmachines is represented,
Subscript G represents navigational coordinate system,
Subscript B represents carrier coordinate system,
△ t represent discrete sampling time interval,
C_{B2G}Expression transforms to the direction cosine matrix of navigational coordinate system from carrier coordinate system,
f^{B}(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 N_{m}Individual 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 mN_{m}The 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 k1 moment in navigational coordinate system is represented,
Represent underwater hidingmachine on orientation/range sensor to underwater hidingmachine center lever arm effect compensation rate in carrier The component of coordinate system,
Subscript S represents orientation/range sensor coordinate system,
C_{S2B}The 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 hidingmachine 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 hidingmachine can obtain the relative of environmental characteristic and underwater hidingmachine 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 nonlinear observation function,
X (k) is system state vector, including the position of underwater hidingmachine, 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 hidingmachine state, therefore has：
z_{mi}(k)=h (x_{vehicle}(k),x_{feature_mi}(k))+ν_{mi}(k)
Wherein：
z_{mi}(k) observation of the k moment to the mi environmental characteristic is represented,
x_{vehicle}(k) state of the underwater hidingmachine at the k moment is represented,
x_{feature_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 hidingmachine 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：
x_{vehicle}(k) k moment underwater hidingmachine states, including underwater hidingmachine position, speed and posture are represented,
x_{feature}(k) state of environmental characteristic is represented, because the position that the state of environmental characteristic only includes environmental characteristic is believed Breath, Gu x_{feature}(k) the actual position for representing environmental characteristic,
f_{vehicle}() 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  k1)=f (x (k1  k1))
Wherein：
X (k  k1) is the onestep prediction quantity of state to the k moment according to the system mode at k1 moment,
X (k1  k1) is the system state estimation value at k1 moment,
P_{cov}(k  k1) is the prediction covariance to the k moment according to the system mode at k1 moment,
P_{cov}(k1  k1) is the state covariance estimate at k1 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：v_{innov}(k)=z (k)h (x (k  k1))
Filtering gain：
State estimation：X (k  k)=x (k  k1)+K_{gain}(k)·v_{innov}(k)
State covariance：
Wherein：
v_{innov}(k) it is new breath,
Z (k) is k moment observations,
H (x (k  k1)) predicted value is observed for the k moment,
K_{gain}(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,
P_{cov}(k  k) is the system mode covariance at k moment；
Step 3.6：The pos obtained in SLAM algorithms from x (k  k)_{vehicle}As 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 pureinertial 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)
Priority Applications (1)
Application Number  Priority Date  Filing Date  Title 

CN201410320791.6A CN104075715B (en)  20140707  20140707  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)  20140707  20140707  A kind of underwater navigation localization method of Combining with terrain and environmental characteristic 
Publications (2)
Publication Number  Publication Date 

CN104075715A CN104075715A (en)  20141001 
CN104075715B true CN104075715B (en)  20170901 
Family
ID=51597151
Family Applications (1)
Application Number  Title  Priority Date  Filing Date 

CN201410320791.6A CN104075715B (en)  20140707  20140707  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)
Publication number  Priority date  Publication date  Assignee  Title 

CN105333869A (en) *  20151104  20160217  天津津航计算技术研究所  Unmanned reconnaissance aerial vehicle synchronous positioning and picture compositing method based on selfadaption EKF 
CN105424036B (en) *  20151109  20180213  东南大学  A kind of inexpensive underwater hidingmachine terrain aided inertia combined navigation localization method 
CN105547300A (en) *  20151230  20160504  航天恒星科技有限公司  Allsource navigation system and method used for AUV (Autonomous Underwater Vehicle) 
CN106052688B (en) *  20160808  20190108  西安电子科技大学  Inertial navigation system speed accumulation error correcting method based on terrain contour matching 
CN107167126B (en) *  20170331  20190920  大鹏高科（武汉）智能装备有限公司  A kind of autonomous type underwater robot Combinated navigation method and system 
CN107132521B (en) *  20170516  20191206  哈尔滨工程大学  method for judging correctness of terrain matching result in BSLAM (binarycoded decimal motion) 
CN107727096A (en) *  20170915  20180223  哈尔滨工程大学  AUV terrain match localization methods based on the screening of effective node 
CN108344999B (en) *  20180109  20200811  浙江大学  Sonar map construction and repositioning method for underwater robot navigation 
CN108318034B (en) *  20180109  20200522  浙江大学  AUV docking navigation method based on sonar map 
CN108362281A (en) *  20180224  20180803  中国人民解放军61540部队  A kind of Long baselines underwater submarine matching navigation method and system 
CN108592916B (en) *  20180420  20200807  杭州电子科技大学  Multinavigation map positioning and navigation method of suspended autonomous underwater vehicle 
RU2709100C1 (en) *  20180619  20191216  Федеральное государственное бюджетное учреждение науки Специальное конструкторское бюро средств автоматизации морских исследований Дальневосточного отделения Российской академии наук  Method of determining location of underwater object 
CN109443343A (en) *  20180913  20190308  安徽优思天成智能科技有限公司  A kind of Target Tracking System 
CN109186610A (en) *  20181015  20190111  哈尔滨工程大学  A kind of robust BSLAM method of AUV terrain match navigation 
Family Cites Families (2)
Publication number  Priority date  Publication date  Assignee  Title 

CN101788295A (en) *  20100226  20100728  南京信息工程大学  Combined navigation system of smallscale underwater vehicle and method thereof 
CN103542851B (en) *  20131104  20160323  东南大学  A kind of submarine navigation device assisting navigation localization method based on underwater topography elevation database 

2014
 20140707 CN CN201410320791.6A patent/CN104075715B/en active IP Right Grant
Also Published As
Publication number  Publication date 

CN104075715A (en)  20141001 
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 inflight coarse alignment  
Georgy et al.  Lowcost threedimensional navigation solution for RISS/GPS integration using mixture particle filter  
Webster et al.  Advances in singlebeacon onewaytraveltime 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 realtime 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 synchronousclock onewaytraveltime 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 fullparameter navigation method based on sequence image and reference image matching  
Jiancheng et al.  Study on innovation adaptive EKF for inflight 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 MEMSbased 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's independent navigation computation based on double MEMSIMU  
GebreEgziabher et al.  MAV attitude determination by vector matching  
CN104181574B (en)  A kind of SINS/GLONASS integrated navigation filtering system and method  
Meduna et al.  Closedloop terrain relative navigation for AUVs with noninertial 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 