CN108562287A - A kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter - Google Patents
A kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter Download PDFInfo
- Publication number
- CN108562287A CN108562287A CN201810013855.6A CN201810013855A CN108562287A CN 108562287 A CN108562287 A CN 108562287A CN 201810013855 A CN201810013855 A CN 201810013855A CN 108562287 A CN108562287 A CN 108562287A
- Authority
- CN
- China
- Prior art keywords
- particle
- navigation
- measurement
- state
- error
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
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
-
- 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manufacturing & Machinery (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
The present invention provides a kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter, first, establishes the state-space model based on inertial navigation site error and the measurement model based on multibeam echosounding sonar；Then, one-step prediction particle update is carried out by initial state distribution and state-space model, adjustment number of particles is distributed according to predicted state using KLD sampling techniques, obtains prediction particle collection；When there is multi-beam measurements arrival, joint pressure depth meter fathoms, inertial navigation indicating positions and underwater reference number map interpolating function, carries out particle by measurement model and measures update；Finally, by measuring updated particle collection and weight, the estimation that criterion carries out aircraft site error is minimized using mean square error, the error of estimation is modified inertial navigation indicating positions.The present invention can improve navigation real-time while ensureing underwater topography assisting navigation precision.
Description
Technical field
The present invention relates to a kind of Terrain-aided Underwater Navigations based on adaptively sampled particle filter, belong to underwater boat
Row device independent navigation field.
Background technology
Currently, submarine navigation device principle navigation system is inertial navigation system (INS) mostly.INS passes through inertial sensor part
Acceleration and posture are measured, by determining speed, position and the course of submarine navigation device to the integral of time.INS can be with
It is independent to be navigated using carrier equipment of itself, without being contacted with the external world, with autonomous, continuous, offer is led in real time
The ability for information of navigating, but while working long hours will produce larger position cumulative errors, if this error is not added with timely
It corrects, will likely result in the mission failure of submarine navigation device, or even jeopardize inherently safe.Global positioning system (GPS) is one
Kind comprehensive, round-the-clock, all the period of time, high-precision satellite navigation system can the problem of accumulation at any time there is no position error
To be used for correcting INS position errors well.But due to the absorption that seawater is strong to electromagnetic wave, submarine navigation device can only be determined
Phase floats to the water surface to carry out positioning amendment using GPS.However this floating correcting mode for aircraft concealment extremely
It is unfavorable, and when deep ocean work, more time and energy consumption are needed, especially when there is ice sheet on sea, the obstruction of ice sheet is more
Make this mode that can not use.Relative to electromagnetic signal, acoustical signal can propagate larger distance under water, therefore, under water
Aircraft using acoustic transmitter and beacon (or transponder) without that in the case of emerging, can be navigated, i.e. sound
Learn navigation.Acoustic navigation positioning system such as Long baselines, short baseline and ultra short baseline locating system, in the absence of between cumulative errors,
Positioning accuracy is higher, but its need additional sonar array lay or lash ship support, greatly reduce the motor-driven of submarine navigation device
Property and concealment, and also there is certain limitation in its zone of action, easily by artificial or natural interference.
Underwater topography can be used as a kind of good supplementary guiding information source as earth build-in attribute.Work as underwater navigation
When device navigates by water, the terrain detection sensor of itself prestowage can be utilized to obtain underwater topography data information in real time, by these data
Information is matched with stored underwater reference number topographic map, can estimate the exact position of submarine navigation device, is come with this
Correct INS cumulative errors.This underwater topography assisting navigation technical error is not accumulated with the increase of hours underway or distance to go
Tired or diverging, and aircraft can be carried out assisting navigation, be had without rising from the water surface with side operation, the terrain information that lateral dominance measures
Effect avoids insufficient existing for the above GPS and acoustic navigation.As a kind of unconventional air navigation aid, underwater topography assisting navigation
Meet well submarine navigation device for a long time, autonomous, safe and high-precision navigation request, there is very wide application
Foreground.
Invention content
It is provided a kind of based on adaptive the purpose of the invention is to improve submarine navigation device navigation accuracy and real-time
Sample the Terrain-aided Underwater Navigation of particle filter.
The object of the present invention is achieved like this：Steps are as follows：
(1) it using inertial navigation site error as state, using multibeam echosounding sonar as bathymetric surveying sensor, builds
The state-space model and measurement model of vertical underwater topography assisting navigation；
(2) it is distributed according to initial position error state, generates primary, one-step prediction grain is carried out by state-space model
Son update obtains prediction particle collection, and in one-step prediction particle renewal process, using KLD sampling techniques to number of particles into
Row adjustment；
(3) when there is multi-beam measurements arrival, the position of depth, inertial navigation instruction that joint pressure depth measures
With underwater reference number map interpolating function, particle is carried out by measurement model and measures update, obtains measuring updated particle collection
And corresponding weight；
(4) by measuring updated particle collection and weight, criterion is minimized using mean square error and carries out aircraft position mistake
The estimation of difference, the error of estimation is modified inertial navigation indicating positions.
The invention also includes some such structure features：
1. the underwater topography assisting navigation state-space model and measurement model established in step (1) are：
δxk=δ xk-1+wk
In formula：δxkFor k moment aircraft north orientations and east orientation site error, wkFor state procedure noise；zkFor a N-dimensional to
Amount indicates that multi-beam sounds the depth of the water and measures aircraft away from the sum of water surface depth with pressure sensor, and N indicates multi-beam one-shot measurement
Wave beam number；For inertial navigation indicating positions；H () is map water depth change function, vkTo measure noise；γkIt indicates
The corresponding position compensation amount of each wave beam under navigational coordinate system；When there is no position compensation item γkWhen, then measurement model, which corresponds to, passes
The simple beam measurement model of system, at this time zkFor one-dimensional scalar.
2. step (2) is specifically：
According to initial state distribution p (δ x0), generate primaryAnd corresponding normalizing
Change weightsThen particle one-step prediction update is carried out by state-space model, is adopted using KLD by the particle of the Weight generated
Sample technology obtains one-step prediction particle collection Sk, KLD sampling steps include：
Initialization：It is empty set S to enable new prediction particle assemblyk:=Φ, sampling total number of particles n=0, KLD particle number nχ
=0, chest number b=0；
Resampling：From the particle collection of Weighted CoefficientsResampling obtains equal weights particle collection
Forecast updating：ByEqual weights, which are obtained, according to state-space model newly samples particlePopulation
Increase n=n+1, and new particle is put into prediction particle assembly
Judge to predict whether particle falls into new chest, if it is not, then returning to resampling steps；
If it is, increasing chest number b=b+1, and calculates required KLD samplings population and be：
Wherein, ε is the KLD thresholdings of setting, and δ is standardized normal distribution quantile；
The magnitude relationship between sampling population and calculated KLD sampling populations is judged, if sampling population is less than
KLD samplings calculate population n ＜ nχ, then continue to sample back to resampling steps；Otherwise end loop obtains KLD and adopts
Like-particles collection
3. the measurement update in step (3) refers to that multi-beam sonar is combined to measure oblique distance and field angle, pressure depth measurement
Measure depth, attitude transducer measurement roll and pitch, inertial navigation indicating positions and map interpolation of the aircraft apart from horizontal plane
Function carries out measurement update by measurement model, obtains new particle collection and corresponding weights：
First, roll and pitch is measured by attitude transducer and multi-beam sonar measurement oblique distance and field angle is converted into carrier
Under coordinate system wave beam footprint laterally away from, it is longitudinal away from the height water-bed with aircraft distance, then, plus the measurement of pressure depth meter
Depth constitutes the measuring value of actual measurement；Then, site error and posture pass represented by inertial navigation indicating positions, each particle
Sensor measures course and finds out each particle in the corresponding multi-beam interpolation water depth value in map coordinates system position；Finally, reality is utilized
The measuring value of survey and the corresponding multi-beam interpolation water depth value of each particle calculate likelihood functionCarry out particle weights
It updates and normalizes
4. step (4) is specifically：
Normalize according to last particle collection and its accordingly weightsUsing minimum mean square error criterion,
Obtain estimation and the covariance estimation of aircraft site error：
Compared with prior art, the beneficial effects of the invention are as follows：It is of the present invention a kind of based on adaptively sampled particle
The Terrain-aided Underwater Navigation of filtering, using multibeam echosounding sonar to measure underwater topography information, use is adaptively sampled
Particle filter estimates inertial navigation site error, to the inertial navigation indicating positions of correction tape error, compared to it
His traditional auxiliary navigation method has the characteristics that safety, autonomous, high-precision, remote navigation, and adaptively sampled particle is filtered
Wave can adjust the size of number of particles compared to standard particle filtering in real time, help to improve the validity of particle filter and lead
The real-time of boat.
Description of the drawings
Fig. 1 is underwater topography assisting navigation functional block diagram；
Fig. 2 is adaptively sampled particle filter algorithm flow chart；
Fig. 3 is that underwater topography refers to landform isogram and simulating realistic flight path；
Fig. 4 is corresponding water depth ratio immediately below real trace；
Fig. 5 a and Fig. 5 b are that primary is that 500 adaptively sampled particle filters and standard particle filter navigation error respectively
With population variation Simulation results comparison；
Fig. 6 a and Fig. 6 b are that primary is that 100 adaptively sampled particle filters and standard particle filter navigation error respectively
With population variation Simulation results comparison.
Specific implementation mode
Present invention is further described in detail with specific implementation mode below in conjunction with the accompanying drawings.
In conjunction with Fig. 1 to Fig. 6 b, a kind of Terrain-aided Underwater Navigation based on adaptive particle filter of the invention, packet
Include following steps：
Step (1) is sensed using inertial navigation site error as state, using multibeam echosounding sonar as bathymetric surveying
Device establishes the state-space model and measurement model of underwater topography assisting navigation；
Step (2) is distributed according to initial position error state, generates primary, and it is pre- to carry out a step by state-space model
Particle update is surveyed, prediction particle collection is obtained, and in one-step prediction particle renewal process, using KLD sampling techniques to population
Mesh is adjusted；
Step (3) when there is multi-beam measurements arrival, joint pressure depth measure depth, inertial navigation instruction
Position and underwater reference number map interpolating function carry out particle by measurement model and measure update, obtain measuring updated grain
Subset and corresponding weight；
Step (4) minimizes criterion by measuring updated particle collection and weight, using mean square error and carries out aircraft position
The error of estimation is modified inertial navigation indicating positions by the estimation for setting error；
Inertial navigation is as basic reference navigation elements, and output current time, the indicating positions with error was as adaptive
The input of particle filter is sampled, meanwhile, multi-beam sounds the depth of the water in real time, pressure depth meter fathoms, attitude transducer is surveyed
Measuring posture and map interpolating function is also used as filter to input, by setting suitable filtering parameter, filtering output estimation position
Error.Whenever having new measuring value, filter can carry out a site error estimation, be accomplished that and continuously lead in real time
Boat, as shown in Figure 1.
Step (1) establishes underwater topography assisting navigation state-space model and measurement model is as follows：
δxk=δ xk-1+wk
Wherein, δ xkFor k moment aircraft north orientations and east orientation site error, wkFor state procedure noise.zkFor a N-dimensional to
Amount indicates that multi-beam sounds the depth of the water and measures aircraft away from the sum of water surface depth with pressure sensor, and N indicates multi-beam one-shot measurement
Wave beam number；For inertial navigation indicating positions；H () is map water depth change function, vkTo measure noise；γkIt indicates
The corresponding position compensation amount of each wave beam under navigational coordinate system；When there is no position compensation item γkWhen, then measurement model, which corresponds to, passes
The simple beam measurement model of system, at this time zkFor one-dimensional scalar.
According to the underwater topography assisting navigation model of above-mentioned foundation it is found that state model is linear, due to the height of landform
Spend non-linear, measurement model is nonlinear, is a nonlinear state Eq problem.Using Bayesian iteration estimation side
Method, the Posterior probability distribution for obtaining site error state are：
p(δxk|Z1:k-1)=∫ p (δ xk|δxk-1)p(δxk-1Z1:k-1)dδxk-1
Wherein, p (zk|δxk) it is likelihood function, p (δ xk|δxk-1) it is a step transfering probability distribution.
Using Minimum Mean Squared Error estimation criterion, then site error state estimation and covariance are estimated as follows：
Step (2) is according to initial state distribution p (δ x0), generate primaryAnd
Corresponding normalization weightsThen particle one-step prediction update is carried out by state-space model, by the particle of the Weight generated
One-step prediction particle collection S is obtained using KLD sampling techniquesk, KLD sampling steps are as follows：
Initialization：It is empty set S to enable new prediction particle assemblyk:=Φ, sampling total number of particles n=0, KLD particle number nχ
=0, chest number b=0；
Resampling：From the particle collection of Weighted CoefficientsResampling obtains equal weights particle collection
Forecast updating：WithEqual weights, which are obtained, according to state-space model newly samples particlePopulation
Increase, n=n+1, and new particle is put into prediction particle assembly
Judge：Whether prediction particle falls into new chest, if it is not, then returning to resampling steps；If it is, increasing
Chest number b=b+1, and calculate required KLD and sample population
Wherein, ε is the KLD thresholdings of setting, and δ is standardized normal distribution quantile；
The size between sampling population and calculated KLD sampling populations is judged, if sampling population is adopted less than KLD
Sample calculates population n ＜ nχ, then continue to sample back to resampling steps；Otherwise end loop obtains KLD sampling particles
Collection
The measurement of step (3) is updated to：Oblique distance is measured in conjunction with multi-beam sonar and field angle, pressure depth meter measure navigation
Depth, attitude transducer measurement roll and pitch, inertial navigation indicating positions and map interpolating function of the device apart from horizontal plane, by
Measurement model carries out measurement update, obtains new particle collection and corresponding weights.
First, roll and pitch is measured by attitude transducer and multi-beam sonar measurement oblique distance and field angle is converted into carrier
Under coordinate system wave beam footprint laterally away from, it is longitudinal away from and the water-bed height of aircraft distance, then, plus pressure depth measurement
Depth is measured, the measuring value of actual measurement is constituted；Then, site error and posture represented by inertial navigation indicating positions, each particle
Sensor measurement course finds out each particle in the corresponding multi-beam interpolation water depth value in map coordinates system position；Finally, it utilizes
The measuring value of actual measurement and the corresponding multi-beam interpolation water depth value of each particle calculate likelihood functionCarry out particle power
Value is updated and is normalized
Step (4) is according to last particle collection and its normalizes weights accordinglyUsing MMSE criterion,
Obtain estimation and the covariance estimation of aircraft site error
To adaptively sampled particle filter and standard particle filtering, Terrain-aided Navigation application has carried out emulation examination under water
Test analysis.Certain lake underwater topography is measured as topographic map is referred to using boat-carrying multibeam echosounding sonar, and the depth of water is at 10 to 60 meters
Between, map grid resolution ratio is 5 meters.Assuming that aircraft depthkeeping at the uniform velocity navigates by water, apart from horizontal plane depth 5m, headway is
2m/s, simulating realistic track are straight line, and sampling interval 0.01s, total hours underway is 600s, and Fig. 3 gives underwater ginseng
Examine landform isogram and real trace.
Using simplified inertial navigation error model, it is assumed that inertial navigation velocity error be constant value velocity error, the north to
With east to velocity error be constant value 0.1m/s, it is 50m that north orientation and east orientation, which is arranged, in initial position error, and sample frequency is
100Hz.The emulation of multibeam echosounding sonar to measure is according to setting list ping wave beam covering of the fans angle of release, numbers of beams, actual position and ground
Shape interpolating function determines；Take angularly pattern, according to wave beam angle of release and numbers of beams determine each wave beam footprint correspond to laterally away from
With longitudinal direction away from, it is assumed here that the numbers of beams of single ping 127,120 degree of angle of release；Then, passed through by corresponding wave beam footprint point
Bilinear interpolation function obtains the map interpolation depth of water of a ping, white Gaussian noise is added on its basis, as the more of emulation
Wave beam depth-determining sonar sounds the depth of the water.Fig. 4 gives the water depth ratio immediately below real trace.
It can be repeated in the same grid since multi-beam measures wave beam footprint, when carrying out terrain match, not
It is sounded the depth of the water a little using whole, but 127 wave beams of method pair of uniform sampling is used to carry out sub-sampling, choose 41 wave beams
Terrain match navigation is carried out as measuring.50 illiteracies are carried out to two methods of adaptive particle filter and standard particle filtering respectively
Special Carlow (MC) l-G simulation test is weighed navigation accuracy using position root-mean-square error (RMSE), is calculated as follows：
Wherein M indicates that MC simulation times, k indicate filtering renewable time.
In conjunction with Fig. 5, it is auxiliary to analyze the landform of adaptive particle filter and standard particle filtering when primary number is 500
Help navigation performance.It can be seen that from Fig. 5 a when initial population is 500, adaptive particle filter and standard particle filtering are fixed
Position navigation accuracy is higher, is not in filtering divergence phenomenon, this is because 500 populations approximation state probability well enough
Distribution, to which adaptively sampled particle filter and standard particle filtering have preferable navigation accuracy.But it can from Fig. 5 b
Go out, filtered compared to standard particle, population used in adaptive particle filter just declines when filtering starts convergence, finally exists
Filtering maintains 100 populations when reaching stable state.Less population mean it is less for each iteration the time it takes,
This means that adaptive particle filter is ensureing same navigation performance, using less particle, there is preferably navigation
Real-time.
In conjunction with Fig. 6, analyze adaptive particle filter and standard particle filtering primary it is several 100 when underwater topography
Assisting navigation error and population variation.It can be seen that from Fig. 6 a when initial population is 100, standard particle filtering navigation
Precision is poor, it may appear that the phenomenon that diverging, the navigation accuracy of adaptive particle filter is able to maintain that good precision, and divergence-free
Phenomenon.This is because start when primary number it is very little, can not well approximation state probability distribution, the particle of standard
Filtering in entire filtering population be always maintained at it is constant, so cause navigation accuracy decline even dissipate；And it is adaptive
Particle filter, which should be sampled, can quickly adjust number of particles so that the enough approximation state probability distribution of number of particles, to protect
It has demonstrate,proved navigation not dissipate, has improved navigation accuracy.It can also be evident that from Fig. 6 b, when filtering initial, adaptively adopt
Like-particles filtering number of particles is adjusted to rapidly greater number, when filtering starts convergence, is gradually reduced, and is finally reached stable state dimension
It holds in 100 particles or so, and standard particle filtering population maintains always 100.Therefore, this illustrates adaptive particle filter phase
Than being filtered in standard particle, it is more advantageous to guarantee navigation validity and real-time.
1.50 Monte Carlo simulation test process times of table
In conjunction with table 1, in the case of analyzing different primary numbers, adaptively sampled particle filter and standard particle filtering
50 Monte Carlo simulation test process times.As can be seen from the table, with the increase of primary number, standard particle filter
Time used in wave also proportionally increases, however, for adaptively sampled particle filter, processing time used is slight
It increased, can adjust always approximation probability point is come with minimum number of particles this is mainly due to adaptive particle filter
Cloth, processing time greatly reduces in this, but loses seldom navigation accuracy.Therefore, it is filtered compared to standard particle, adaptively
Sampling particle filter can improve the validity and navigation real-time of particle filter.
To sum up, the present invention provides a kind of Terrain-aided Underwater Navigations based on adaptively sampled particle filter.Water
Lower aircraft utilizes the multibeam echosounding sonar to measure underwater topography of prestowage, will be measured using adaptively sampled particle filter method
Landform is compared with underwater reference number map, the site error of submarine navigation device is recursively estimated, to inertial navigation system
Output position is modified.First, it establishes the state-space model based on inertial navigation site error and is based on multibeam echosounding
The measurement model of sonar；Then, one-step prediction particle update is carried out by initial state distribution and state-space model, using KLD
Sampling technique is distributed adjustment number of particles according to predicted state, obtains prediction particle collection；When there is multi-beam measurements arrival, connection
Resultant pressure depth gauge fathoms, inertial navigation indicating positions and underwater reference number map interpolating function, by measurement model into
Row particle measures update；Finally, by measuring updated particle collection and weight, criterion is minimized using mean square error and is navigated by water
The error of estimation is modified inertial navigation indicating positions by the estimation of device site error.The present invention can ensure under water
Navigation real-time is improved while Terrain-aided Navigation precision.
Claims (5)
1. a kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter, it is characterised in that：Steps are as follows：
(1) using inertial navigation site error as state, using multibeam echosounding sonar as bathymetric surveying sensor, water is established
The state-space model and measurement model of lower Terrain-aided Navigation；
(2) it is distributed according to initial position error state, generates primary, one-step prediction particle is carried out more by state-space model
Newly, prediction particle collection is obtained, and in one-step prediction particle renewal process, number of particles is adjusted using KLD sampling techniques
It is whole；
(3) when there is multi-beam measurements arrival, joint pressure depth measure depth, inertial navigation instruction position and water
Lower reference number map interpolating function carries out particle by measurement model and measures update, obtains measuring updated particle collection and phase
The weight answered；
(4) by measuring updated particle collection and weight, criterion is minimized using mean square error and carries out aircraft site error
Estimation, the error of estimation is modified inertial navigation indicating positions.
2. a kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter according to claim 1,
It is characterized in that：The underwater topography assisting navigation state-space model and measurement model of foundation are in step (1)：
δxk=δ xk-1+wk
In formula：δxkFor k moment aircraft north orientations and east orientation site error, wkFor state procedure noise；zkFor a N-dimensional vector,
It indicates that multi-beam sounds the depth of the water and measures aircraft away from the sum of water surface depth with pressure sensor, N indicates multi-beam one-shot measurement wave
Beam number；For inertial navigation indicating positions；H () is map water depth change function, vkTo measure noise；γkExpression is being led
The corresponding position compensation amount of each wave beam under boat coordinate system；When there is no position compensation item γkWhen, then measurement model corresponds to tradition
Simple beam measurement model, z at this timekFor one-dimensional scalar.
3. a kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter according to claim 2,
It is characterized in that：Step (2) is specifically：
According to initial state distribution p (δ x0), generate primaryI=1,2 ... N, and accordingly normalize weightsThen particle one-step prediction update is carried out by state-space model, KLD sampling techniques is used by the particle of the Weight generated
Obtain one-step prediction particle collection Sk, KLD sampling steps include：
Initialization：It is empty set S to enable new prediction particle assemblyk:=Φ, sampling total number of particles n=0, KLD particle number nχ=0,
Chest number b=0；
Resampling：From the particle collection of Weighted CoefficientsResampling obtains equal weights particle collection
Forecast updating：ByEqual weights, which are obtained, according to state-space model newly samples particlePopulation increases n
=n+1, and new particle is put into prediction particle assembly
Judge to predict whether particle falls into new chest, if it is not, then returning to resampling steps；
If it is, increasing chest number b=b+1, and calculates required KLD samplings population and be：
Wherein, ε is the KLD thresholdings of setting, and δ is standardized normal distribution quantile；
The magnitude relationship between sampling population and calculated KLD sampling populations is judged, if sampling population is adopted less than KLD
Sample calculates population n ＜ nχ, then continue to sample back to resampling steps；Otherwise end loop obtains KLD sampling particles
Collection
4. a kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter according to claim 3,
It is characterized in that：Measurement update in step (3) refers to that multi-beam sonar is combined to measure oblique distance and field angle, the measurement of pressure depth meter
Depth, attitude transducer measurement roll and pitch, inertial navigation indicating positions and map interpolation letter of the aircraft apart from horizontal plane
Number, carries out measurement update by measurement model, obtains new particle collection and corresponding weights：
First, roll and pitch is measured by attitude transducer and multi-beam sonar measurement oblique distance and field angle is converted into carrier coordinate
The lower wave beam footprint of system laterally away from, it is longitudinal away from the height water-bed with aircraft distance, then, measured deeply plus pressure depth meter
Degree, constitutes the measuring value of actual measurement；Then, site error and posture sensing represented by inertial navigation indicating positions, each particle
Device measures course and finds out each particle in the corresponding multi-beam interpolation water depth value in map coordinates system position；Finally, actual measurement is utilized
Measuring value and the corresponding multi-beam interpolation water depth value of each particle, calculate likelihood functionCarry out particle weights more
Newly and normalize
5. a kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter according to claim 4,
It is characterized in that：Step (4) is specifically：
Normalize according to last particle collection and its accordingly weightsUsing minimum mean square error criterion, obtain
The estimation of aircraft site error and covariance estimation：
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810013855.6A CN108562287A (en) | 2018-01-08 | 2018-01-08 | A kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810013855.6A CN108562287A (en) | 2018-01-08 | 2018-01-08 | A kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN108562287A true CN108562287A (en) | 2018-09-21 |
Family
ID=63529684
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810013855.6A Pending CN108562287A (en) | 2018-01-08 | 2018-01-08 | A kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108562287A (en) |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110726415A (en) * | 2019-10-21 | 2020-01-24 | 哈尔滨工程大学 | Self-adaptive underwater multi-beam synchronous positioning and mapping method |
| CN110763234A (en) * | 2019-10-15 | 2020-02-07 | 哈尔滨工程大学 | Submarine topography matching navigation path planning method for underwater robot |
| CN110906928A (en) * | 2019-11-22 | 2020-03-24 | 哈尔滨工程大学 | Particle filter underwater track tracking method based on terrain gradient fitting |
| CN111220146A (en) * | 2019-12-10 | 2020-06-02 | 哈尔滨工程大学 | Underwater terrain matching and positioning method based on Gaussian process regression learning |
| CN111238483A (en) * | 2020-02-12 | 2020-06-05 | 上海海事大学 | Low-power-consumption terrain auxiliary navigation system and method based on SIR particle filtering method |
| CN112445244A (en) * | 2020-11-09 | 2021-03-05 | 中国科学院沈阳自动化研究所 | Target searching method for multiple autonomous underwater robots |
| CN112653992A (en) * | 2019-10-12 | 2021-04-13 | 中国科学院声学研究所 | Mobile formation relative self-positioning method without clock synchronization |
| CN112926618A (en) * | 2019-12-05 | 2021-06-08 | Aptiv技术有限公司 | Method and system for determining initial self-pose for self-positioning initialization |
| CN112946660A (en) * | 2021-01-28 | 2021-06-11 | 西北工业大学 | Multi-beam forward-looking sonar-based simultaneous positioning and mapping method |
| CN113008223A (en) * | 2021-03-03 | 2021-06-22 | 哈尔滨工程大学 | Seabed terrain synchronous positioning and mapping method based on particle filtering and mapping optimization |
| CN113189985A (en) * | 2021-04-16 | 2021-07-30 | 南京大学 | Partially observable driving planning method based on adaptive particle and belief filling |
| CN116295538A (en) * | 2023-05-24 | 2023-06-23 | 河海大学 | USBL (universal serial bus) installation error calibration method based on improved particle filtering |
| CN116341283A (en) * | 2023-05-19 | 2023-06-27 | 汇杰设计集团股份有限公司 | Underwater topography simulation method and system based on deep water silt sampling |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102426018A (en) * | 2011-08-19 | 2012-04-25 | 北京航空航天大学 | Terrain auxiliary navigation method based on mixture of terrain contour matching (TERCOM) algorithm and particle filtering |
| CN103542851A (en) * | 2013-11-04 | 2014-01-29 | 东南大学 | Underwater vehicle aided navigation positioning method based on underwater topography elevation database |
| CN104330084A (en) * | 2014-11-13 | 2015-02-04 | 东南大学 | Neural network assisted integrated navigation method for underwater vehicle |
| CN104390646A (en) * | 2014-09-15 | 2015-03-04 | 东南大学 | Position matching method for underwater vehicle terrain aided inertial navigation system |
| CN105424036A (en) * | 2015-11-09 | 2016-03-23 | 东南大学 | Terrain-aided inertial integrated navigational positioning method of low-cost underwater vehicle |
| US20160167750A1 (en) * | 2012-10-19 | 2016-06-16 | Ixblue | System and method for the navigation of a movable vehicle, suitable for determining and displaying a safe navigation zone |
| CN106017460A (en) * | 2016-05-20 | 2016-10-12 | 东南大学 | Terrain-assisted inertial-navigation tight-combination underwater vehicle navigation and positioning method |
| CN106123850A (en) * | 2016-06-28 | 2016-11-16 | 哈尔滨工程大学 | AUV prestowage multibeam sonar underwater topography mapping modification method |
| CN107124159A (en) * | 2017-04-27 | 2017-09-01 | 鲁东大学 | A kind of implementation method of the particle filter based on adaptive KLD cassette lengths |
| CN107314768A (en) * | 2017-07-06 | 2017-11-03 | 上海海洋大学 | Underwater terrain matching aided inertial navigation localization method and its alignment system |
-
2018
- 2018-01-08 CN CN201810013855.6A patent/CN108562287A/en active Pending
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102426018A (en) * | 2011-08-19 | 2012-04-25 | 北京航空航天大学 | Terrain auxiliary navigation method based on mixture of terrain contour matching (TERCOM) algorithm and particle filtering |
| US20160167750A1 (en) * | 2012-10-19 | 2016-06-16 | Ixblue | System and method for the navigation of a movable vehicle, suitable for determining and displaying a safe navigation zone |
| CN103542851A (en) * | 2013-11-04 | 2014-01-29 | 东南大学 | Underwater vehicle aided navigation positioning method based on underwater topography elevation database |
| CN104390646A (en) * | 2014-09-15 | 2015-03-04 | 东南大学 | Position matching method for underwater vehicle terrain aided inertial navigation system |
| CN104330084A (en) * | 2014-11-13 | 2015-02-04 | 东南大学 | Neural network assisted integrated navigation method for underwater vehicle |
| CN105424036A (en) * | 2015-11-09 | 2016-03-23 | 东南大学 | Terrain-aided inertial integrated navigational positioning method of low-cost underwater vehicle |
| CN106017460A (en) * | 2016-05-20 | 2016-10-12 | 东南大学 | Terrain-assisted inertial-navigation tight-combination underwater vehicle navigation and positioning method |
| CN106123850A (en) * | 2016-06-28 | 2016-11-16 | 哈尔滨工程大学 | AUV prestowage multibeam sonar underwater topography mapping modification method |
| CN107124159A (en) * | 2017-04-27 | 2017-09-01 | 鲁东大学 | A kind of implementation method of the particle filter based on adaptive KLD cassette lengths |
| CN107314768A (en) * | 2017-07-06 | 2017-11-03 | 上海海洋大学 | Underwater terrain matching aided inertial navigation localization method and its alignment system |
Non-Patent Citations (1)
| Title |
|---|
| 张静远等: "水下地形辅助导航技术的研究与应用进展", 《国防科技大学学报》 * |
Cited By (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112653992B (en) * | 2019-10-12 | 2022-01-07 | 中国科学院声学研究所 | Mobile formation relative self-positioning method without clock synchronization |
| CN112653992A (en) * | 2019-10-12 | 2021-04-13 | 中国科学院声学研究所 | Mobile formation relative self-positioning method without clock synchronization |
| CN110763234A (en) * | 2019-10-15 | 2020-02-07 | 哈尔滨工程大学 | Submarine topography matching navigation path planning method for underwater robot |
| CN110726415A (en) * | 2019-10-21 | 2020-01-24 | 哈尔滨工程大学 | Self-adaptive underwater multi-beam synchronous positioning and mapping method |
| CN110726415B (en) * | 2019-10-21 | 2023-09-29 | 哈尔滨工程大学 | Self-adaptive underwater multi-beam synchronous positioning and mapping method |
| CN110906928A (en) * | 2019-11-22 | 2020-03-24 | 哈尔滨工程大学 | Particle filter underwater track tracking method based on terrain gradient fitting |
| CN112926618B (en) * | 2019-12-05 | 2024-03-26 | Aptiv制造管理服务公司 | Method and system for determining initial self-pose of self-positioning initialization |
| CN112926618A (en) * | 2019-12-05 | 2021-06-08 | Aptiv技术有限公司 | Method and system for determining initial self-pose for self-positioning initialization |
| CN111220146A (en) * | 2019-12-10 | 2020-06-02 | 哈尔滨工程大学 | Underwater terrain matching and positioning method based on Gaussian process regression learning |
| CN111220146B (en) * | 2019-12-10 | 2022-10-14 | 哈尔滨工程大学 | Underwater terrain matching and positioning method based on Gaussian process regression learning |
| CN111238483A (en) * | 2020-02-12 | 2020-06-05 | 上海海事大学 | Low-power-consumption terrain auxiliary navigation system and method based on SIR particle filtering method |
| CN112445244A (en) * | 2020-11-09 | 2021-03-05 | 中国科学院沈阳自动化研究所 | Target searching method for multiple autonomous underwater robots |
| CN112946660A (en) * | 2021-01-28 | 2021-06-11 | 西北工业大学 | Multi-beam forward-looking sonar-based simultaneous positioning and mapping method |
| CN113008223A (en) * | 2021-03-03 | 2021-06-22 | 哈尔滨工程大学 | Seabed terrain synchronous positioning and mapping method based on particle filtering and mapping optimization |
| CN113008223B (en) * | 2021-03-03 | 2022-10-28 | 哈尔滨工程大学 | Seabed terrain synchronous positioning and mapping method based on particle filtering and mapping optimization |
| CN113189985A (en) * | 2021-04-16 | 2021-07-30 | 南京大学 | Partially observable driving planning method based on adaptive particle and belief filling |
| CN116341283A (en) * | 2023-05-19 | 2023-06-27 | 汇杰设计集团股份有限公司 | Underwater topography simulation method and system based on deep water silt sampling |
| CN116295538A (en) * | 2023-05-24 | 2023-06-23 | 河海大学 | USBL (universal serial bus) installation error calibration method based on improved particle filtering |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108562287A (en) | A kind of Terrain-aided Underwater Navigation based on adaptively sampled particle filter | |
| CN105823480B (en) | Underwater moving target location algorithm based on single beacon | |
| CN106886024B (en) | Deep-sea multi-beam sound ray precise tracking method | |
| CN102749622B (en) | Multiwave beam-based depth-sounding joint inversion method for sound velocity profile and seafloor topography | |
| JP5039392B2 (en) | Underwater vehicle and positioning method for underwater vehicle | |
| US20220155069A1 (en) | System and method for wave prediction | |
| CN110208812A (en) | Unmanned vehicles seabed dimensional topography detection device and method partly latent | |
| US8004930B2 (en) | Methods and systems for determining coordinates of an underwater seismic component in a reference frame | |
| CN111596333B (en) | Underwater positioning navigation method and system | |
| CN103744098A (en) | Ship's inertial navigation system (SINS)/Doppler velocity log (DVL)/global positioning system (GPS)-based autonomous underwater vehicle (AUV) combined navigation system | |
| McPhail et al. | Range-only positioning of a deep-diving autonomous underwater vehicle from a surface ship | |
| CA2256964C (en) | Method of locating hydrophones | |
| CN109375646A (en) | AUV docking recycling autonomous navigation method based on FMSRUPF algorithm | |
| CN108469620A (en) | Bathymetric surveying method suitable for killing pests with irradiation shallow sea water | |
| CN111220146B (en) | Underwater terrain matching and positioning method based on Gaussian process regression learning | |
| XU | Using gravity anomaly matching techniques to implement submarine navigation | |
| CN109738902A (en) | A kind of autonomous acoustic navigation method of underwater high-speed target with high precision based on sync beacon mode | |
| CN110221278A (en) | A kind of SAS movement compensation method based on multi sensor combination | |
| Rice et al. | Measuring the water level datum relative to the ellipsoid during hydrographic survey | |
| Arnesen et al. | Probabilistic localization and mapping of flexible underwater structures using octomap | |
| Wang et al. | Measurement error analysis of multibeam echosounder system mounted on the deep-sea autonomous underwater vehicle | |
| Jalving et al. | Positioning accuracy for the hugin detailed seabed mapping uuv | |
| Song et al. | Underwater slope measurement using a tilted multibeam sonar head | |
| KR100650518B1 (en) | A specifying method and system for settling position of pru | |
| Morozov et al. | Methodical aspects of the application of acoustic doppler current profilers in the black sea |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20180921 |
|
| RJ01 | Rejection of invention patent application after publication |