CN108225310B - Gravity-assisted navigation track planning method - Google Patents
Gravity-assisted navigation track planning method Download PDFInfo
- Publication number
- CN108225310B CN108225310B CN201711414134.8A CN201711414134A CN108225310B CN 108225310 B CN108225310 B CN 108225310B CN 201711414134 A CN201711414134 A CN 201711414134A CN 108225310 B CN108225310 B CN 108225310B
- Authority
- CN
- China
- Prior art keywords
- track
- inertial navigation
- gravity
- error
- random
- 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.)
- Active
Links
- 230000005484 gravity Effects 0.000 title claims abstract description 153
- 238000000034 method Methods 0.000 title claims abstract description 75
- 230000006978 adaptation Effects 0.000 claims abstract description 62
- 238000004364 calculation method Methods 0.000 claims abstract description 40
- 238000005259 measurement Methods 0.000 claims abstract description 32
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 22
- 230000003044 adaptive effect Effects 0.000 claims abstract description 17
- 230000008569 process Effects 0.000 claims abstract description 13
- 238000011835 investigation Methods 0.000 claims abstract description 9
- 238000013016 damping Methods 0.000 claims description 49
- 230000001133 acceleration Effects 0.000 claims description 29
- 238000012216 screening Methods 0.000 claims description 18
- 230000002159 abnormal effect Effects 0.000 claims description 16
- 238000012937 correction Methods 0.000 claims description 11
- 238000010276 construction Methods 0.000 claims description 10
- 238000013519 translation Methods 0.000 claims description 8
- 238000005070 sampling Methods 0.000 claims description 5
- 238000002474 experimental method Methods 0.000 claims description 3
- 238000004088 simulation Methods 0.000 claims description 3
- 238000011156 evaluation Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 5
- 230000004044 response Effects 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 239000000969 carrier Substances 0.000 description 2
- 230000009189 diving Effects 0.000 description 2
- 238000005295 random walk Methods 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000002349 favourable effect Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 230000000737 periodic effect Effects 0.000 description 1
- 230000008092 positive effect Effects 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Images
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 non-inertial 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
The invention relates toThe gravity assisted navigation track planning method is technically characterized by comprising the following steps: the method comprises the following steps: step 1, acquiring a gravity anomaly background image of an adaptation area and an error standard deviation g thereof through investigationm(ii) a Investigating and researching performance parameters of each component device of the inertia/gravity combined navigation system; step 2, constructing and traversing various possible random tracks by utilizing a search-traversal track algorithm; step 3, calculating the standard deviation g of gravity measurement errors on all the alternative trackscAnd inertial navigation positioning error divergence values during matching; and 4, calculating the estimated matching positioning accuracy of all the alternative tracks, and taking the alternative track with the highest matching positioning accuracy as the planned track in the adaptive area. According to the method, the gravity measurement error model is used for simulating the underwater gravity measurement process, the inertial navigation positioning error calculation method is used for simulating the working condition of the carrier navigating underwater, and the actual underwater matching working condition can be reproduced, so that the reliability of the screened-out flight path is ensured.
Description
Technical Field
The invention belongs to the technical field of inertia/gravity combined navigation, relates to a flight path planning method, and particularly relates to a gravity-assisted navigation flight path planning method.
Background
The gravity-assisted navigation track planning refers to the step of screening a track suitable for carrying out inertia/gravity combined navigation in the ocean, and the combined navigation system carries out matching positioning calculation on the track, so that high-precision position information can be given, and the readjustment and correction of inertial navigation are realized.
At present, a common flight path planning method such as an a-algorithm mostly uses water depth data and gravity feature statistics as evaluation criteria for flight path screening. Most of the algorithms use manually set empirical indexes as threshold values of evaluation standards, the randomness is strong, the matching and positioning accuracy of the screened tracks cannot be guaranteed, and the reliability is poor. And the quantitative relation between the matching positioning precision and the gravity characteristic statistic cannot be given, and the intuitiveness is poor.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provide an adaptive flight path planning method which is visual and reliable, can comprehensively consider the underwater navigation working condition of a carrier and takes the matching positioning precision as a screening evaluation standard.
The technical problem to be solved by the invention is realized by adopting the following technical scheme:
a gravity-assisted navigation track planning method comprises the following steps:
and 4, step 4: t of all alternative tracksp、gm、gcAnd GL is substituted into a matching positioning accuracy pre-estimation formula to calculate the pre-estimation matching positioning accuracy of all the alternative tracks, and the alternative track with the highest matching positioning accuracy is used as a planning track in the adaptation area.
Moreover, the setting of the inertial navigation mode, the working mode of the gravimeter and the navigation mode of the carrier in step 1 includes the following steps:
(1) using inertial navigation under the horizontal damping working condition to guide the carrier to directly navigate at a constant speed;
(2) measuring by using a gravimeter working under a horizontal damping working condition;
(3) and (4) carrying out gravity anomaly correction by using information given by inertial navigation under the horizontal damping working condition.
Moreover, the performance parameters of each component device of the investigation inertia/gravity integrated navigation system in step 1 include the following:
(1) element accuracy index of target inertial navigation: delta Ax、ΔAxr、ΔAy、ΔAyrRespectively representing the equivalent constant value zero offset and the equivalent random error of the east and north horizontal adding tables of the inertial navigation;x、xr、y、yrrespectively representing equivalent constant drift and equivalent random errors of the east and north horizontal gyros of the inertial navigation;
(2) horizontal damping parameters of the target inertial navigation: w is a1~w10The parameters are horizontal damping network coefficients and can be obtained by investigating inertial navigation performance;
(3) element accuracy index of the target gravimeter: delta Axg、ΔAxgr、ΔAyg、ΔAygrRespectively representing the equivalent constant value zero offset and the equivalent random error of the gravity meter east and north horizontal adding tables;xg、xgr、yg、ygrrespectively representing equivalent constant drift and equivalent random errors of the horizontal gyros in the east direction and the north direction of the gravity meter;
(4) horizontal damping parameters of the target gravimeter: w is ag1~wg10The parameters can be obtained by investigating the gravimeter for the horizontal damping network coefficient.
(5) The depth sounding precision index of the target depth sounding potential measuring instrument is as follows: h is the error of the potential measuring instrument, and the value of the error can be found from the operation manual of the potential measuring instrument;
(6) the speed measurement precision index of the target Doppler log is as follows:Vry、Vrxthe value of the error represents the error of the north-direction speed and the east-direction speed given by the Doppler log and can be found from an instruction manual of the Doppler log.
Moreover, the step 2 adopts a search-traversal track algorithm to complete the construction and traversal of various possible random tracks, and comprises the following specific steps: the method comprises the following steps:
(1) normalizing the shape of the adaptation region:
traversing all edge points of a certain adaptation area, calculating the distance Dis between any two points, wherein the maximum value of Dis is the diameter of a circumscribed circle of the adaptation area, two edge points corresponding to the maximum distance Dis are tangent points of the adaptation area and the circumscribed circle, the average value of coordinates of the two tangent points is the coordinate of the center of the circumscribed circle, and the area in the circumscribed circle range is regarded as the normalized adaptation area;
(2) determining whether the size of the adaptation area is sufficient:
when the diameter Dis of the normalized adaptation area is greater than 2 × Y nautical miles, the gravity anomaly characteristics at the real position are sufficiently rich, and therefore, the search traversal of the flight path can be performed on the adaptation area. When the diameter Dis of the normalized adaptive area is less than 2X Y sea, the real track of the carrier may run out of the adaptive area, and the matching and positioning accuracy cannot be guaranteed, so that the adaptive area cannot be used and is directly discarded;
(3) moving error bands traverse the adaptation region:
in order to fully utilize the gravity anomaly characteristics in the adaptation area, the random track is positioned on the diameter of the circumscribed circle, and at the moment, according to a position relation graph between the circumscribed circle and the random track, when X is less than Dis-2X Y, the random track can move along the diameter of the circumscribed circle, X represents the length of the random track, B represents the translation position of the random track on the diameter of the circumscribed circle, and K represents the course of the random track with the diameter of the circumscribed circle; with the traversal of the possible value of B, the error band can move along the diameter of the circumscribed circle on a certain course, and finally the adaptive area near the diameter is covered; then traversing the possible values of K, wherein the error band can completely cover the circular adaptation area along with the rotation of the diameter of the circumscribed circle, thereby realizing the traversal of the random track of the error band;
(4) search-logical order of traversal:
in the searching-traversing process, the values of the random track length X and the speed V are firstly solidified, and the length is X0Speed of flight V0Translating an error band corresponding to the random track on the diameter of the adaptation area and rotating along with the diameter, and solving an inertial navigation positioning error according to an inertial navigation positioning error solving method; and calculating the total length as X by adopting a gravity characteristic abundance degree calculation method0Speed of V0The GL value of the random track is taken as the random track with the maximum GL as the length X0Speed of V0Alternative flight path under the condition and calculating T of the flight pathp=X0/V0;
Further modifying the random track length X and the track speed V, screening out the random track with the maximum GL under any track length and any track speed according to the method to form an alternative track set, and calculating the T of all the alternative trackspAnd (4) completing the construction and traversal of various possible random tracks.
In addition, the step (4) of the step 2 of solving the inertial navigation positioning error according to the inertial navigation positioning error solving method specifically comprises the following steps:
① first sets the random track length to X0At sea, a navigational speed of V0Section;
solving longitude and latitude coordinates of the random track;
setting an initial course K of search traversal0Degree, initial translation position B0Sea lining; let the course be K0The intersection point of the diameter of the adaptation area and the circumscribed circle is translated along the diameter of the circumscribed circle B0And obtaining the starting point of the random track in the sea. Starting from the starting point of the random track, the course is K0Is cut to a length X0The segment in the sea is the required random track; considering that the resolving frequency of the matching positioning system is 1HZ, a linear interpolation algorithm is adopted to interpolate the random track into Tp=X0/V0The position sequence of each sampling point can obtain the random time when the carrier sails along the random trackThe longitude and latitude coordinates of;
solving the rotation and acceleration sensed by the gyroscope and the accelerometer when the carrier navigates along the random track;
firstly, calculating the acceleration and rotation information sensed by a target inertial navigation adding table and a gyro when a carrier runs on the random track, wherein the acceleration and rotation information is shown as the following formula:
ωx=-vy/R
in the above formula, Ax、Ay、AzRepresenting the acceleration sensed by the accelerometer under the geographic coordinate system; omegax、ωy、ωzRepresenting the rotation angular velocity sensed by the gyroscope under a geographic coordinate system, and being the amount to be resolved;
vx=V0*cos(K0)、vy=V0*sin(K0)、respectively representing the east speed, the north speed and the latitude of the random track;
omega represents the rotational angular velocity of the earth, and R represents the radius of the earth and is a constant.
g represents the local absolute gravity value and can be obtained by combining the latitude of the random track with a WGS-84 ellipsoid model;
vzrepresenting the vertical velocity of the carrier, can be obtained from data recorded from offshore experiments.
Solving inertial navigation positioning errors;
after the rotation and the acceleration sensed by the inertial element on the random track are obtained, the inertial navigation output can be simulated on a computer by using a basic equation of a horizontal damping inertial navigation system, the longitude and the latitude of the inertial navigation output are the real track of the carrier, and the longitude and the latitude of the inertial navigation output are subtracted from the longitude and the latitude of the random track to obtain the inertial navigation positioning error. The basic equation of the horizontal damping inertial navigation system is as follows:
Apx=Ax-β*g+ΔAx+ΔAxr
Apy=Ay+α*g+ΔAy+ΔAyr
Apx、Apythe method comprises the steps of adding east acceleration and north acceleration output by a meter to inertial navigation level under a geographic coordinate system, α and β respectively represent attitude error angles of an inertial navigation inertial platform deviating from the geographic level, gamma represents a heading error calculated by inertial navigation solution, and Vcx、Vcy、λcRespectively represent the east velocity of the carrier calculated by inertial navigation,North speed, latitude, longitude; w is acx、wcy、wczRepresenting the torque applied to the gyroscope, wherein the parameter is a to-be-solved quantity;
Vry、Vrxrepresenting the north and east speeds given by a Doppler log, by the velocity v at random trackx、vyVelocity measurement error V of upper-superposed Doppler logry、VrxObtaining;
under the condition of internal horizontal damping working condition of inertial navigation work, K 10, B-1, C-D-E-0, and K when inertial navigation works in external horizontal damping mode1=1,A=B=1,C=D= E=0;
ua~ufIntermediate variables in the resolving process;
the equation set only consists of a first-order linear homogeneous differential equation and a first-order equation, and can be solved by means of a Runge-Kutta method; wherein λc、φcThe true longitude and latitude of the carrier are obtained, and the true longitude and latitude of the carrier are subtracted from the longitude and latitude of the random track, so that the inertial navigation positioning error can be obtained.
Moreover, the gravity feature richness degree calculation method adopted in the step (4) of the step 2 calculates the total length as X0Speed of V0The specific steps of the GL value of the random track comprise:
firstly, translating a real track;
any one real track generated by a certain random track is translated to east and west 0.5 nautical miles along the latitude line, and a bilinear interpolation method is utilized to obtain the gravity abnormal value sequences on the two tracks, wherein the gravity abnormal value sequences are respectively as follows:
any one real track generated by a certain random track is translated to the south and the north by 0.5 nautical miles along the meridian line, and the gravity abnormal value sequences on the two tracks can be obtained by utilizing a bilinear interpolation method, wherein the sequences are respectively as follows:
secondly, calculating the gravity abnormal gradient on the real track:
on the real track, the gradient sequence Tx in the longitude direction and the gradient sequence Ty in the latitude direction are shown as follows:
and thirdly, calculating GL of the random track:
for any random track, the number of real tracks generated by simulation through an inertial navigation positioning error solution method is 100, and the average value of the gradient variances in all the real tracks in the longitude direction can be defined as follows:
the average of the gradient variances in all true track latitudes can be defined as follows:
wherein, Txi、TyiRespectively is a longitude and latitude gradient sequence on the ith real track.
Then the gravity anomaly feature richness GL of the corresponding random track can be expressed as:
by combining the gravity characteristic abundance calculation method and the inertial navigation positioning error calculation method, the total length X can be calculated0Speed of flight V0The GL value of the random track, the random track with the maximum GL is the length X0Speed of V0Candidate tracks under conditions.
And further modifying the random track length X and the track speed V, and screening out the random track with the maximum GL under any track length and any track speed condition according to the method to form an alternative track set.
Furthermore, the gravity measurement error model used in step 3 is as follows:
in the formula, g represents a local absolute gravity value and can be obtained by combining the latitude of an alternative track with a WGS-84 ellipsoid model;representing the latitude of the alternative track; v. ofeEast-direction speed representing an alternative track; v. ofnRepresenting the northbound speed of the alternate track. Omega represents the rotational angular velocity of the earth, R represents the radius of the earth, A1= 978032.67714,B1=0.00193185138639,C1= -0.00669437999013, D1-0.3086, constant;
in the above formula, gcFor gravity measurement errors to be solved, the error sources are divided into three categories: h is the error of the potential measuring instrument, and the value of the error can be found from the operation manual of the potential measuring instrument; a isE、aN、ve、vnThe error of information given for inertial navigation, α, β are the misalignment angle of the gravity meter platform, for the latter two kinds of error sources, the basic equation of horizontal damping inertial navigation can be used to solve and calculate by combining the Runge-Kutta method, all the error sources are substituted into the formula to obtain gcThe numerical solution of (c). The standard deviation g of the measured gravity error during the matching periodc=std(gc)。
In addition, the inertial navigation positioning error solution method in step 3 is used to calculate the inertial navigation positioning error divergence values during matching on all candidate tracks under the parameters and working mode conditions set in step 1, as shown in the following formula:
in the above formula, the subscript t ∈ [ t1,t2],t1And t2Representing the start-stop time of the match.λctRepresenting the longitude and latitude on the real track of the carrier calculated by the inertial navigation positioning error calculation method in the step 2;λtrepresenting alternative track latitudes and longitudes. The divergence value of inertial navigation positioning error during matching is max (t)。
Moreover, the matching positioning accuracy estimation formula of step 4 is shown as follows:
calculating g of all alternative tracks by using the gravity measurement error model given in the step 3 and the inertial navigation positioning error divergence value calculation method during the matchingcAnd combining the GL and T of all the selected tracks obtained by calculation in the step 2pAnd g obtained by investigationmAnd substituting the predicted matching positioning accuracy into a matching positioning accuracy prediction formula to obtain the predicted matching positioning accuracy of all the alternative tracks, wherein the track with the highest predicted matching positioning accuracy in all the alternative tracks is the finally obtained planned track.
The invention has the advantages and positive effects that:
1. the invention is an adaptive track planning method which comprehensively considers the underwater navigation working condition of a carrier and takes the matching positioning precision as a screening track evaluation index, can directly provide the track with the highest matching positioning precision, and has strong intuition.
2. The screening process fully considers the characteristics of the underwater navigation of the carrier, utilizes the gravity measurement error model to simulate the underwater gravity measurement process, utilizes the inertial navigation positioning error calculation method to simulate the underwater navigation working condition of the carrier, and can reproduce the actual matching working condition, thereby ensuring the reliability of the screened track.
3. The invention adopts a search-traversal track algorithm to enable the screening process to completely cover the adaptation area, thereby ensuring the optimality of the screening result.
Drawings
FIG. 1 is a graph of the logarithmic amplitude-frequency response characteristic of the horizontal damping network of the present invention;
FIG. 2 is a schematic diagram of an inertial navigation error circle according to the present invention;
FIG. 3 is a schematic diagram of an inertial navigation error band of the present invention;
FIG. 4 is a diagram illustrating adaptation region normalization according to the present invention;
FIG. 5 is a diagram of the spatial location of the circumscribed circle and error band of the present invention;
FIG. 6 is a graph of the positional relationship between the circumscribed circle and the planned flight path of the present invention;
FIG. 7 is a process flow diagram of the track planning algorithm of the present invention;
FIG. 8 is a graph of inertial navigation positioning error divergence values during matching according to the present invention.
Detailed Description
The embodiments of the invention will be described in further detail below with reference to the accompanying drawings:
the background technology of the flight path planning is a gravity field database adaptability analysis and evaluation method, a gravity matching precision pre-estimation formula obtained by the evaluation method is utilized,
as shown in the following formula:
in order to establish a track planning algorithm, a matching precision pre-estimation formula needs to be analyzed to find out parameters influencing matching precision, wherein the parameters appearing in the formula comprise:
matching time length TpT is related to the length X of the planned track, the speed Vp=X/V;
During the matching period, the inertial navigation positioning error divergence value is related to the length X and the speed V of a planned flight path and can be obtained by calculation by using an inertial navigation positioning error resolving method;
during matching, representing the abundance degree of the gravity abnormal features on the real track of the carrier by the gravity gradient standard deviation GL on the real track of the carrier, wherein the values of the gravity abnormal features are related to the length X, the course K, the position B and the speed V of a planned track, and can be obtained by utilizing a search-traversal track algorithm combined with an inertial navigation positioning error resolving method and a gravity feature abundance degree calculating method;
standard deviation g of measured gravity error during matchingcAnd the correlation between the length X of the planned flight path, the flight speed V, whether the shape of the flight path is a straight line or not and whether the flight speed is stable or not can be obtained by calculation by using a gravity measurement error model.
Therefore, the factors such as the position, the shape, the course, the length, the speed and the like of the flight path all affect the final matching positioning precision. The invention is based on a matching precision pre-estimation formula, aims to obtain a high-precision matching positioning position, develops a track planning scheme design, and provides a method for determining factors such as a planned track position, a shape, a course, a length, a speed and the like.
A gravity assisted navigation track planning method, as shown in fig. 7, includes the following steps:
(1) setting the working modes of inertial navigation and a gravimeter and the navigation mode of a carrier:
the realization of gravity matching depends on high-precision actually measured gravity anomaly information which is measured by a relative gravimeter. Besides the gravity anomaly information, the signals output by the relative gravimeter also contain interference items such as additional centrifugal acceleration, Coriolis acceleration, normal gravity value and the like generated by the motion of the carrier, and accordingly, normal field correction and Hertefsh correction are required to be carried out on the output signals of the relative gravimeter so as to complete the separation of the interference items. These corrections need to be based on carrier latitude information, horizontal velocity information. When the carrier navigates underwater, the reference information of speed and position of the carrier, which is continuous, stable and long-term, is generally given by inertial navigation, so that the accuracy of the inertial navigation output information directly influences the calculation accuracy of the interference item, and further influences the accuracy of the actually measured gravity anomaly.
In order to improve the accuracy of information given by inertial navigation, the inertial navigation needs to be set under a horizontal damping working condition, and the logarithmic amplitude-frequency response characteristic of a horizontal damping network is shown in fig. 1. The damping network mainly has the following two functions: on one hand, the network adds a pole on an inertial navigation level resolving channel, so that Schuler periodic oscillation in information given by inertial navigation can be eliminated; on the other hand, the network can damp high-frequency impulse noise introduced into the inertial navigation system by factors such as a gyro drift random walk part, a random walk part with zero offset of a meter, sea waves and the like, and eliminate accumulation of inertial navigation on noise response, so that the accuracy of information given by inertial navigation is improved. However, the introduction of the horizontal damping network destroys the schuler adjustment condition of inertial navigation, and in the state of violent maneuvering of the carrier, the information given by the inertial navigation is overshot, and the precision of the inertial navigation is reduced, so that the carrier needs to be kept in a uniform-speed direct navigation state under the inertial navigation guidance in order to exert the suppression effect of the damping network on inertial navigation errors.
In addition, the relative gravimeter is also an inertial navigation system in nature. The maneuvering condition and the horizontal damping condition of the uniform speed direct navigation are also favorable for improving the horizontal attitude precision relative to the electromechanical platform or the mathematical platform of the gravimeter so as to create a stable working environment for the gravity sensor.
In order to ensure the matching precision by combining the theoretical analysis, the carrier and the equipment carried by the carrier need to work in the following states:
using inertial navigation under the horizontal damping working condition to guide the carrier to directly navigate at a constant speed;
measuring by using a gravimeter working under a horizontal damping working condition;
and (4) performing gravity anomaly correction by using information given by the horizontal damping inertial navigation.
(2) The performance parameters of each component device of the inertia/gravity combined navigation system comprise the following contents:
element accuracy index of target inertial navigation: delta Ax、ΔAxr、ΔAy、ΔAyrRespectively representing the equivalent constant value zero offset and the equivalent random error of the east and north horizontal adding tables of the inertial navigation;x、xr、y、yrrespectively representing equivalent constant drift and equivalent random errors of the east and north horizontal gyros of the inertial navigation;
horizontal damping parameters of the target inertial navigation: w is a1~w10The parameters are horizontal damping network coefficients, and can be obtained by investigating inertial navigation performance.
Element accuracy index of the target gravimeter: delta Axg、ΔAxgr、ΔAyg、ΔAygrRespectively representing the equivalent constant value zero offset and the equivalent random error of the gravity meter east and north horizontal adding tables;xg、xgr、yg、ygrrespectively representing equivalent constant drift and equivalent random errors of the horizontal gyros in the east direction and the north direction of the gravity meter;
horizontal damping parameters of the target gravimeter: wg1~Wg10The parameters can be obtained by investigating the gravimeter for the horizontal damping network coefficient.
The depth sounding precision index of the target depth sounding potential measuring instrument is as follows: h is the error of the potential measuring instrument, and the value of the error can be found from the operation manual of the potential measuring instrument;
the speed measurement precision index of the target Doppler log is as follows: vry、VrxRepresenting errors in north and east velocity given by a Doppler logThe value of the difference can be found from the Doppler log instruction manual.
(1) the searching-traversing track algorithm in the step 2 is realized by the following steps:
the method comprises the following steps that a carrier navigates along a planned track by means of inertial navigation guidance during underwater matching, the true track of the carrier deviates from the planned track due to the existence of inertial navigation positioning errors, and the matching positioning precision is directly influenced by considering the gravity gradient standard deviation GL on the true track of the carrier, so that the GL value of the true track in the area close to the track needs to be calculated in the track planning process and serves as an evaluation standard of the adaptability of the true track; and then, a planned flight path is screened out from any flight path by a search traversal method. It can be seen from this that: the suitability of any track is determined by the GL value of the true track in the vicinity of the track: the search traversal of the flight path is essentially the search traversal of the area near the flight path.
The implementation method of the search-traversal track algorithm is given in the following steps.
1) Normalizing the shape of the adaptation region:
generally, before planning a track, a candidate sea area is artificially divided into a plurality of parts, GL in each part is calculated, and a part with a larger GL is defined as an adaptation area. For the adaptation region with irregular shape, in order to facilitate search traversal, the shape of the adaptation region needs to be normalized: traversing all edge points of a certain adaptation area, calculating the distance Dis between any two points, wherein the maximum value of Dis is the diameter of a circumscribed circle of the adaptation area, two edge points corresponding to the maximum distance Dis are tangent points of the adaptation area and the circumscribed circle, and the average value of coordinates of the two tangent points is the coordinate of the center of the circumscribed circle. The area within the circumscribed circle range is regarded as the normalized adaptive area, as shown in fig. 4, the method for solving the circumscribed circle of the original adaptive area ensures that the normalized adaptive area completely covers the original adaptive area with any shape, and simultaneously ensures that the gravity field characteristics can be fully utilized during track planning.
2) Determining whether the size of the adaptation area is sufficient:
and (3) taking the inertial navigation indication position as the center of a circle and the maximum value Y of the positioning error of the inertial navigation as the radius to make an error circle, wherein the actual position of the carrier is certainly included in the circle at any moment in the matching process, as shown in fig. 2, the maximum value Y of the inertial navigation positioning error is taken as the radius, the inertial navigation positioning error circle is made by taking the inertial navigation indication position as the center of a circle, and the actual position of the carrier is certainly positioned in the error circle. During matching, the carrier needs to navigate directly under inertial navigation guidance, as the matching is performed, the inertial navigation indicated position is elongated from a point to a straight line, and accordingly, an error circle near the inertial navigation indicated position is also elongated to form a rectangular error band, the width of the error band is 2 × Y nautical miles, at this time, the true track of the carrier still lies in the error band near the inertial navigation indicated track, as shown in fig. 3, during matching, the carrier needs to navigate directly under inertial navigation guidance, as the matching is performed, the inertial navigation indicated position is elongated from a point to a straight line, and accordingly, the error circle near the inertial navigation indicated position is also elongated to form a rectangular error band, the width of the error band is 2 × Y, and the true position of the carrier is certainly located inside the error band.
In order to ensure that the real trajectory can be located within the adaptation zone, it is ensured that the error band falls completely within the adaptation zone. When the diameter Dis of the normalized adaptation area is larger than 2X Y sea, the normalized adaptation area can completely cover an error band, so that the gravity anomaly characteristics on the real position are sufficiently abundant, and the track search traversal can be carried out on the adaptation area. When the diameter Dis of the normalized adaptation area is smaller than 2 × Y, the real track of the carrier may run out of the adaptation area, and the matching and positioning accuracy cannot be guaranteed, so that the adaptation area cannot be used and is directly discarded.
3) Moving error bands traverse the adaptation region:
according to the navigation mode of the carrier during the matching, the inertial navigation indication track during the matching is coincident with the planned track, so that the real track of the carrier is positioned in an error band near the planned track. Therefore, the error band can represent the area which contains the true track and is near the flight path, and the search traversal of the flight path and the area near the flight path is essentially the search traversal of the inertial navigation positioning error band.
When the diameter Dis of the circumscribed circle is greater than 2 × Y nautical miles, the spatial position relationship between the circumscribed circle and the error band is shown in fig. 5, where the distance between the inertial navigation instruction track (planned track) and the center of the circumscribed circle is d. Because the length of the optimal flight path cannot be determined in advance before flight path planning, in order to fully utilize the gravity anomaly characteristics in the adaptation area and improve the success rate of flight path planning to the maximum extent, the area of an error band falling into the adaptation area is tried to be ensured to be the maximum, and the error band falling into the area of a circumscribed circle, as shown in the following formula:
and (4) calculating the derivative of the S to the d, wherein when the d is equal to 0, the area S obtains the maximum value, which indicates that the area of the error band falling into the adaptation area is the maximum when the inertial navigation indication track (planning track) is positioned on the diameter of the circumscribed circle. Thus, the planned flight path will be searched on the circumscribed circle diameter and, correspondingly, the search traversal of the error band will be developed along the circumscribed circle diameter of the adaptation zone.
When the planned flight path is on the circumscribed circle diameter, the positional relationship between the circumscribed circle and the planned flight path is shown in fig. 6, in the figure, X represents the length of the random flight path, when X < Dis-2 × Y, the random flight path can move along the circumscribed circle diameter, B represents the translation position of the random flight path on the circumscribed circle diameter, and K represents the heading of the circumscribed circle diameter (random flight path). As the possible value of B is traversed, the error band will move along the circumscribed circle diameter on a certain heading, eventually covering the adaptation zone near the diameter. Then, the possible value of K is traversed, and the error band can completely cover the circular adaptation area along with the rotation of the diameter of the circumscribed circle, so that the traversal of the error band (random track) is realized. Generally, the traversal step length of the heading K is 1 degree, and the traversal step length of the position B is 1 nautical mile.
4) Search-logical order of traversal:
considering that the random track needs to be completely determined by four parameters, namely length X, speed V, course K and translation position B, and the influence modes of the parameters on the matching positioning accuracy are different, in the searching-traversing process, the screening sequence of the parameters needs to be reasonably arranged, and the optimal values of the track parameters need to be determined one by one.
As can be seen from the discussion in the background art section, the track length X and the speed V not only affect GL, but also affect; and the translation position B and the heading K of the track only affect GL but not GL. According to the principle of gradual screening, the algorithm firstly eliminates the influence on the matching positioning precision, namely the length X of the curing track and the speed V, and the length X is used as the length0Speed of flight V0The error band corresponding to the random track is translated on the diameter of the adaptation area and rotates along with the diameter, 100 real tracks of carriers are constructed in the random error band corresponding to the random track with any course K and any intercept B by using an inertial navigation positioning error calculation method given by the third section, GL on all the 100 real tracks of the carriers is calculated by using a gravity feature abundance degree calculation method given by the fifth section, and the average value of the 100 real tracks GL is used as an evaluation index of the adaptability of the random track. The random track with the maximum GL in all random tracks is the random track with the length of X0Marine and a speed of flight of V0On the premise, the alternative track in the adaptation area, T at this timep=X0/V0。
Further modifying the random track length X and the track speed V, screening out the random track with the maximum GL under any track length and any track speed according to the method to form an alternative track set, and calculating the T of all the alternative trackspX/V. Thereby completing the search and traversal of various possible random trajectories. The random track length X can be 20 nautical miles or 30 nautical miles, and the speed V can be 7 knots or 10 knots.
(2) The inertial navigation positioning error calculation method in the step 2 comprises the following concrete implementation steps:
considering that the matching positioning accuracy is directly influenced by the abundance degree of the gravity characteristics on the real track of the carrier, in order to analyze the adaptability of a certain random track, the real track needs to be constructed in the corresponding inertial navigation positioning error band, and the real track can be obtained by superposing the inertial navigation positioning error on the random track. The inertial navigation positioning error solving method comprises the following steps:
firstly, setting the length of a random track as X0At sea, a navigational speed of V0And (4) saving.
1) Solving longitude and latitude coordinates of the random track:
setting an initial course K of search traversal0Degree, initial translation position B0In the sea. Let the course be K0The intersection point of the diameter of the adaptation area and the circumscribed circle is translated along the diameter of the circumscribed circle B0And obtaining the starting point of the random track in the sea. Starting from the starting point of the random track, the course is K0Is cut to a length X0And the segments in the sea are the required random tracks. Considering that the resolving frequency of the matching positioning system is 1HZ, a linear interpolation algorithm is adopted to interpolate the random track into Tp=X0/V0And obtaining the longitude and latitude coordinates of the carrier at any moment when the carrier navigates along the random track through the position sequence of the sampling points.
2) Solving the rotation and acceleration sensed by a gyro and an accelerometer when the carrier navigates along the random track:
since the inertial navigation system is a nonlinear and time-varying system, an analytical solution of the divergence value of the inertial navigation positioning error during matching cannot be obtained. In order to determine the value, the rotation and the acceleration sensed by the inertial element under the geographic coordinate system need to be solved according to the latitude, the east speed and the north speed of the alternative track. As shown in the following formula:
ωx=-vy/R
in the above formula, Ax、Ay、AzRepresenting the acceleration sensed by the accelerometer under the geographic coordinate system; omegax、ωy、ωzAnd the angular velocity of the rotation sensed by the gyro under the geographic coordinate system is represented as the quantity to be solved.
Omega represents the rotational angular velocity of the earth, and R represents the radius of the earth and is a constant.
g represents the absolute gravity value of the local area, and can be calculated by combining the latitude of the alternative track with a WGS-84 ellipsoid model.
vzRepresenting the vertical velocity of the carrier, can be obtained from data recorded from offshore experiments.
3) Solving the inertial navigation positioning error:
after the rotation and acceleration sensed by the inertial element on the random track are acquired, the inertial navigation output can be simulated on a computer by using the basic equation of the horizontal damping inertial navigation system. Longitude and latitude output by inertial navigation are the real track of the carrier, and can be used for calculating GL of the random track; and subtracting the longitude and the latitude output by the inertial navigation from the longitude and the latitude of the random track to obtain the inertial navigation positioning error. The basic equation of the horizontal damping inertial navigation system is as follows:
Apx=Ax-β*g+ΔAx+ΔAxr
Apy=Ay+α*g+ΔAy+ΔAyr
a in equation set (2)px、ApyThe method comprises the steps of adding east acceleration and north acceleration output by a meter to inertial navigation level under a geographic coordinate system, α and β respectively represent attitude error angles of an inertial navigation inertial platform deviating from the geographic level, gamma represents a heading error calculated by inertial navigation solution, and Vcx、Vcy、λcRespectively representing the east speed, the north speed, the latitude and the longitude of the carrier calculated by inertial navigation; w is acx、
wcy、wczRepresenting the torque applied to the gyro by the inertial navigation system, and the parameter is to be solved.
ΔAx、ΔAxr、ΔAy、ΔAyrRespectively representing the east and north horizontal addition table constant zero offset and random error of the inertial navigation;x、xr、y、yrrespectively representing the constant drift and random error of the east and north horizontal gyros of the inertial navigation; w is a1~w10The parameters can be obtained by investigating inertial navigation for horizontal damping network coefficients.
Vry、VrxThe representation of the north speed and the east speed given by the Doppler velocity meter can be obtained by superposing the velocity measurement error of the Doppler velocity meter on the velocity of the alternative track.
Under the condition of internal horizontal damping working condition of inertial navigation work, K 10, B-1, C-D-E-0, and K when inertial navigation works in external horizontal damping mode1=1,A=B=1,C=D= E=0。
ua~ufAre intermediate variables in the solution process.
The equation set (2) is composed of a first-order linear homogeneous differential equation and a linear equation, and can be solved by means of a Runge-Kutta method. Wherein λc、φcThe true longitude and latitude of the carrier are obtained, and the true longitude and latitude of the carrier are subtracted from the longitude and latitude of the random track, so that the inertial navigation positioning error can be obtained.
Considering that the inertial navigation element error is random, the true position of the carrier during matching may be anywhere within the error band. In order to simulate a real matching working condition, 100 real tracks can be constructed in an error band range corresponding to any random track in a mode of randomly generating element errors during track planning, and the method is used for calculating the gravity anomaly characteristic abundance degree of the random track.
(3) The gravity feature abundance degree calculation method adopted in the step 2 comprises the following steps:
considering that the gravity matching positioning algorithm takes the trend of gravity anomaly as an observed quantity, the fluctuation of the gravity anomaly characteristics directly reflects the richness of the gravity anomaly characteristics, and only the area with severe fluctuation of the gravity anomaly can provide effective observed quantity for the matching positioning algorithm. The fluctuation of the gravity anomaly can be represented by a standard deviation GL of the gravity anomaly gradient, and the bigger the GL is, the stronger the gravity anomaly gradient anisotropy of the gravity field at each position and direction is proved to be, and correspondingly, the more severe the fluctuation of the gravity anomaly is.
On the gravity map, a random track is generated by a search-traversal track algorithm, and the positions on the track are gathered intoConsidering that the submersible vehicle navigates underwater by means of inertial navigation guidance, the real track of the carrier inevitably deviates from the random track, and the real track of the carrier can be constructed by utilizing a horizontal damping inertial navigation basic equation and based on the random track.
Any one real track generated by a certain random track is translated to east and west 0.5 nautical miles along the latitude line, and a bilinear interpolation method is utilized to obtain the gravity abnormal value sequences on the two tracks, wherein the gravity abnormal value sequences are respectively as follows:
any one real track generated by a certain random track is translated to the south and the north by 0.5 nautical miles along the meridian line, and the gravity abnormal value sequences on the two tracks can be obtained by utilizing a bilinear interpolation method, wherein the sequences are respectively as follows:
then, on the real track, the gradient sequence Tx in the longitude direction and the gradient sequence Ty in the latitude direction are shown as follows:
for any random track, the number of real tracks generated by simulation through an inertial navigation positioning error solution method is 100, and the average value of the gradient variances in all the real tracks in the longitude direction can be defined as follows:
the average of the gradient variances in all true track latitudes can be defined as follows:
wherein, Txi、TyiRespectively is a longitude and latitude gradient sequence on the ith real track.
Then the gravity anomaly feature richness GL of the corresponding random track can be expressed as:
by combining the gravity characteristic abundance calculation method and the inertial navigation positioning error calculation method, the total length X can be calculated0Speed of flight V0The GL value of the random track, the random track with the maximum GL is the length X0Speed of V0Candidate tracks under conditions.
And further modifying the random track length X and the track speed V, and screening out the random track with the maximum GL under any track length and any track speed condition according to the method to form an alternative track set.
And step 3: calculating the standard deviation g of gravity measurement errors on all alternative tracks under the conditions of the parameters set in the step 1 and the working modes by using the gravity measurement error model designed by the inventionc(ii) a By using the inertial navigation positioning error calculation method designed by the invention, the inertial navigation positioning error divergence values during the matching period on all alternative tracks under the parameters and working mode conditions set in the step 1 are calculated.
(1) The gravity measurement error model adopted in the step 3 comprises the following contents:
because the gravity matching positioning algorithm takes the fluctuation of the gravity anomaly as the observed quantity, the accumulated error generated by the drift of the gravity sensor before the matching is started can not influence the matching precision; the matching duration is generally not more than 3 hours, and during the period, the accumulated drift error in the output signal of the gravity sensor can not exceed +/-0.05 mgal; in addition, the environment temperature in the submersible is relatively stable, and the temperature of the area near the gravity sensor can be controlled within +/-0.1 ℃ under the environment of the submersible by a temperature control system arranged in the gravimeter, so that the gravity measurement error caused by temperature change can be ignored. After the error is eliminated, the actually measured gravity error can be classified into the following three types according to the source of the error:
under the working state of horizontal damping, the coordinate system of the gravity instrument platform can deviate from the local geographical coordinate system, so that the sensitive axis of the instrument deviates from the direction of the gravity perpendicular line, and absolute gravity measurement errors are generated.
The carrier uses inertial navigation to guide navigation, so that the horizontal acceleration of the carrier inevitably has control errors, and the errors can be coupled into the direction of a gravity sensitive shaft through the attitude error of the gravity meter platform.
And gravity abnormity correction errors caused by horizontal damping inertial navigation speed errors, horizontal damping inertial navigation latitude positioning errors and diving depth errors acquired by a diving instrument.
In this section, modeling description is first performed on the three types of errors, and a gravity measurement error model is established; and then analyzing various error sources contained in the model, and providing a calculation method for solving the numerical solution of the error sources.
For a gravimeter working under a horizontal damping condition, the unit vector in the direction of a sensitive axis is assumed to be usedIt is shown that,suppose that the platform error angles in the east and north directions are respectively:andboth of these angles are small, and thus vectorThe amplitude of the angle between the local geographical coordinate system and the sky direction is as follows:
the measured gravity error caused by the attitude deflection of the gravimeter platform can be expressed as: wherein,the absolute gravity vector at the alternative track can be calculated by combining the latitude of the alternative track with a WGS-84 ellipsoid model. The error value is shown as the right formula:due to the fact thatIs very small and therefore can be approximately consideredAndthe directions are consistent, so that:
the above equation is the measurement error caused by the attitude deflection of the gravimeter platform, and is converted into a scalar form with mgal as a unit as follows:
for a carrier utilizing inertial navigation to guide navigation under the horizontal damping working condition, the eastern acceleration and the north acceleration are respectively assumed as follows:andthe projection of the east acceleration of the carrier on the gravity sensitive axis is:the direction is obtained by the right-hand rule, and the values are as follows: in the formulaIs composed ofFrom the beginning toThe included angle of (c) is positive counterclockwise and negative clockwise. Similarly, the projection of the north acceleration onto the gravity sensitive axis is:the direction is obtained by the right-hand rule, and the values are as follows:in the formulaIs composed ofFrom the beginning toThe included angle of (c) is positive counterclockwise and negative clockwise.
the above equation is the measurement error of the coupling of the horizontal acceleration of the carrier into the gravity sensitive axis, and is converted into a scalar quantity form with mgal as a unit as follows:
g4=(aE*β-aN*α)*105(4)
the ertfsh correction formula, normal field correction formula and vertical acceleration correction formula in mgal are as follows:
wherein A is1=978032.67714,B1=0.00193185138639,
C1=-0.00669437999013,D1T is the carrier height sampling period, generally T is 1s, Ω is the earth rotation angular velocity, R is the earth radius, v is the carrier height sampling period-0.3086eEast speed, v, for alternative tracknFor the north-going speed of the alternate track,and h is the latitude of the alternative track, and h is the carrier submergence depth given by the submergence measuring instrument. By using the above three formulas, respectively deriving the latitude, east speed, north speed and altitude, the following can be obtained:
gh=(h(t-1)+h(t+1)-2*h(t))*105(7)
combining the formulas (3) to (7) to obtain a gravity measurement error model as shown in the following formula:
the error sources in the above formula fall into three categories: h is the error of the potential measuring instrument, and the value of the error can be found from the operation manual of the potential measuring instrument; a isE、aN、ve、vnThe error of information given for inertial navigation, α and β are the misalignment angle of the gravity meter platform, for the latter two kinds of error sources, the basic equation of horizontal damping inertial navigation can be used to solve and calculate by combining the Runge-Kutta method, and all the error sources are substituted into the above formula to obtain gcThe numerical solution of (c). The standard deviation g of the measured gravity error during the matching periodc=std(gc)。
(2) The inertial navigation positioning error divergence value during the matching period in the step 3 is calculated by the following method:
the divergence condition of the inertial navigation positioning error is shown in fig. 8 and is obtained by solving an inertial navigation basic equation by adopting a dragon-Kutta method.
In the above formula, the subscript t ∈ [ t1,t2],t1And t2Representing the start-stop time of the match.λctRepresenting the longitude and latitude on the real track of the carrier calculated by the inertial navigation positioning error calculation method in the step 2;λtrepresenting alternative track latitudes and longitudes. The divergence value of inertial navigation positioning error during matching is max (t)。
And 4, step 4: t of all alternative tracksp、gm、gcAnd GL is substituted into a matching positioning accuracy pre-estimation formula, the pre-estimation matching positioning accuracy of all the alternative tracks can be calculated, and the alternative track with the highest matching positioning accuracy is the planned track in the adaptation area.
The matching positioning accuracy pre-estimation formula of the step 4 is shown as follows:
calculating g of all alternative tracks by using the gravity measurement error model given in the step 3 and the inertial navigation positioning error divergence value calculation method during the matchingcIn a similar manner to that of. Combining GL and T of all selected flight paths obtained by calculation in step 2pAnd g obtained by investigationmSubstituting the estimated matching positioning accuracy into a matching positioning accuracy estimation formula to obtain the estimated matching positioning accuracy of all the alternative tracksAnd (4) predicting a path with the highest matching positioning precision in the alternative paths, namely obtaining a finally obtained planned path.
It should be emphasized that the embodiments described herein are illustrative rather than restrictive, and thus the present invention is not limited to the embodiments described in the detailed description, but also includes other embodiments that can be derived from the technical solutions of the present invention by those skilled in the art.
Claims (8)
1. A gravity-assisted navigation track planning method is characterized by comprising the following steps: the method comprises the following steps:
step 1, acquiring a gravity anomaly background image of an adaptation area and an error standard deviation g thereof through investigationm(ii) a Setting working modes of inertial navigation and a gravimeter and a navigation mode of a carrier; performance parameters of all component equipment of the inertial/gravity combined navigation system are investigated, and the maximum value Y of inertial navigation positioning error during matching is obtained;
step 2, under the conditions of the parameters and the working modes set in the step 1, the construction and traversal of various possible random tracks are completed by utilizing a search-traversal track algorithm: combining an inertial navigation positioning error calculation method and a gravity characteristic abundance degree calculation method, screening out random tracks with most abundant gravity abnormal characteristics GL under the conditions of any length and any navigational speed in an adaptation area, and calculating the matching time length T of the random trackspForming an alternative track set so as to complete the construction and traversal of various possible random tracks;
step 3, calculating the standard deviation g of the gravity measurement errors on all the alternative tracks under the conditions of the parameters and the working modes set in the step 1 by using a gravity measurement error modelc(ii) a Calculating the divergence value of inertial navigation positioning errors during the matching period on all alternative tracks under the conditions of the parameters and the working modes set in the step 1 by using an inertial navigation positioning error calculation method;
step 4, T of all alternative tracksp、gm、gcGL, substituting into a matching positioning accuracy pre-estimation formula, calculating the pre-estimation matching positioning accuracy of all alternative tracks, and using the alternative track with highest matching positioning accuracy as the alternative trackA planned flight path within the adaptation zone;
the matching positioning accuracy pre-estimation formula in the step 4 is shown as the following formula:
calculating g of all alternative tracks by using the gravity measurement error model given in the step 3 and the inertial navigation positioning error divergence value calculation method during the matchingcAnd combining the GL and T of all the selected tracks obtained by calculation in the step 2pAnd g obtained by investigationmAnd substituting the predicted matching positioning accuracy into a matching positioning accuracy prediction formula to obtain the predicted matching positioning accuracy of all the alternative tracks, wherein the track with the highest predicted matching positioning accuracy in all the alternative tracks is the finally obtained planned track.
2. The gravity-assisted navigation track planning method according to claim 1, characterized in that: the setting of the working modes of the inertial navigation instrument and the gravimeter and the navigation mode of the carrier in the step 1 comprises the following contents:
(1) using inertial navigation under the horizontal damping working condition to guide the carrier to directly navigate at a constant speed;
(2) measuring by using a gravimeter working under a horizontal damping working condition;
(3) and (4) carrying out gravity anomaly correction by using information given by inertial navigation under the horizontal damping working condition.
3. A gravity assisted navigation trajectory planning method according to claim 1 or 2, characterized by: the performance parameters of each component device of the investigation inertia/gravity combined navigation system in the step 1 comprise the following contents:
(1) element accuracy index of target inertial navigation: delta Ax、ΔAxr、ΔAy、ΔAyrRespectively representing the equivalent constant value zero offset and the equivalent random error of the east and north horizontal adding tables of the inertial navigation;x、xr、y、yrfor east and north horizontal gyros of inertial navigationEquivalent constant drift and equivalent random error;
(2) horizontal damping parameters of the target inertial navigation: w is a1~w10The parameters are horizontal damping network coefficients and can be obtained by investigating inertial navigation performance;
(3) element accuracy index of the target gravimeter: delta Axg、ΔAxgr、ΔAyg、ΔAygrRespectively representing the equivalent constant value zero offset and the equivalent random error of the gravity meter east and north horizontal adding tables;xg、xgr、yg、ygrrespectively representing equivalent constant drift and equivalent random errors of the horizontal gyros in the east direction and the north direction of the gravity meter;
(4) horizontal damping parameters of the target gravimeter: w is ag1~wg10The parameters are horizontal damping network coefficients and can be obtained by investigating a gravimeter;
(5) the depth sounding precision index of the target depth sounding potential measuring instrument is as follows: h is the error of the potential measuring instrument, and the value of the error can be found from the operation manual of the potential measuring instrument;
(6) the speed measurement precision index of the target Doppler log is as follows: vry、VrxThe value of the error represents the error of the north-direction speed and the east-direction speed given by the Doppler log and can be found from an instruction manual of the Doppler log.
4. A gravity assisted navigation trajectory planning method according to claim 3, characterized by: the step 2 adopts a search-traversal track algorithm to complete the construction and traversal of various possible random tracks, and comprises the following specific steps:
(1) normalizing the shape of the adaptation region:
traversing all edge points of a certain adaptation area, calculating the distance Dis between any two points, wherein the maximum value of Dis is the diameter of a circumscribed circle of the adaptation area, two edge points corresponding to the maximum distance Dis are tangent points of the adaptation area and the circumscribed circle, the average value of coordinates of the two tangent points is the coordinate of the center of the circumscribed circle, and the area in the circumscribed circle range is regarded as the normalized adaptation area;
(2) determining whether the size of the adaptation area is sufficient:
when the diameter Dis of the normalized adaptive area is larger than 2X Y sea, the gravity anomaly characteristics on the real position are abundant enough, so that track search traversal can be carried out on the adaptive area; when the diameter Dis of the normalized adaptive area is less than 2X Y sea, the real track of the carrier may run out of the adaptive area, and the matching and positioning accuracy cannot be guaranteed, so that the adaptive area cannot be used and is directly discarded;
(3) moving error bands traverse the adaptation region:
in order to fully utilize the gravity anomaly characteristics in the adaptation area, the random track is positioned on the diameter of the circumscribed circle, and at the moment, according to a position relation graph between the circumscribed circle and the random track, when X < Dis-2X Y, the random track can move along the diameter of the circumscribed circle, X represents the length of the random track, B represents the translation position of the random track on the diameter of the circumscribed circle, and K represents the course of the random track with the diameter of the circumscribed circle; with the traversal of the possible value of B, the error band can move along the diameter of the circumscribed circle on a certain course, and finally the adaptive area near the diameter is covered; then traversing the possible values of K, wherein the error band can completely cover the circular adaptation area along with the rotation of the diameter of the circumscribed circle, thereby realizing the traversal of the random track of the error band;
(4) search-logical order of traversal:
in the searching-traversing process, the values of the random track length X and the speed V are firstly solidified, and the length is X0Speed of flight V0Translating an error band corresponding to the random track on the diameter of the adaptation area and rotating along with the diameter, and solving an inertial navigation positioning error according to an inertial navigation positioning error solving method; and calculating the total length as X by adopting a gravity characteristic abundance degree calculation method0Speed of V0The GL value of the random track is taken as the random track with the maximum GL as the length X0Speed of V0Alternative flight path under the condition and calculating T of the flight pathp=X0/V0;
Further modifying the random track length X and the speed V, and screening out the maximum GL under the conditions of any track length and any speed according to the methodRandom flight path, forming alternative flight path set, and calculating T of all alternative flight pathspAnd (4) completing the construction and traversal of various possible random tracks.
5. The gravity-assisted navigation track planning method according to claim 4, wherein: the step (4) of the step 2 of solving the inertial navigation positioning error according to the inertial navigation positioning error solving method comprises the following specific steps:
① first sets the random track length to X0At sea, a navigational speed of V0Section;
solving longitude and latitude coordinates of the random track;
setting an initial course K of search traversal0Degree, initial translation position B0Sea lining; let the course be K0The intersection point of the diameter of the adaptation area and the circumscribed circle is translated along the diameter of the circumscribed circle B0Obtaining a starting point of the random track in the sea; starting from the starting point of the random track, the course is K0Is cut to a length X0The segment in the sea is the required random track; considering that the resolving frequency of the matching positioning system is 1HZ, a linear interpolation algorithm is adopted to interpolate the random track into Tp=X0/V0Obtaining longitude and latitude coordinates of the carrier at any moment when the carrier navigates along the random track by using the position sequence of the sampling points;
solving the rotation and acceleration sensed by the gyroscope and the accelerometer when the carrier navigates along the random track;
firstly, calculating the acceleration and rotation information sensed by a target inertial navigation adding table and a gyro when a carrier runs on the random track, wherein the acceleration and rotation information is shown as the following formula:
ωx=-vy/R
in the above formula, Ax、Ay、AzRepresenting the acceleration sensed by the accelerometer under the geographic coordinate system; omegax、ωy、ωzRepresenting the rotation angular velocity sensed by the gyroscope under a geographic coordinate system, and being the amount to be resolved;
vx=V0*cos(K0)、vy=V0*sin(K0)、respectively representing the east speed, the north speed and the latitude of the random track;
omega represents the rotational angular velocity of the earth, R represents the radius of the earth and is a constant;
g represents the local absolute gravity value and can be obtained by combining the latitude of the random track with a WGS-84 ellipsoid model;
vzthe vertical speed of the representative carrier can be obtained from data recorded in a sea-going experiment;
solving inertial navigation positioning errors;
after the rotation and the acceleration sensed by the inertial element on the random track are obtained, the inertial navigation output can be simulated on a computer by using a basic equation of a horizontal damping inertial navigation system, the longitude and the latitude of the inertial navigation output are the real track of the carrier, and the longitude and the latitude of the inertial navigation output are subtracted from the longitude and the latitude of the random track to obtain the inertial navigation positioning error; the basic equation of the horizontal damping inertial navigation system is as follows:
Apx=Ax-β*g+ΔAx+ΔAxr
Apy=Ay+α*g+ΔAy+ΔAyr
Apx、Apythe method comprises the steps of adding east acceleration and north acceleration output by a meter to inertial navigation level under a geographic coordinate system, α and β respectively represent attitude error angles of an inertial navigation inertial platform deviating from the geographic level, gamma represents a heading error calculated by inertial navigation solution, and Vcx、Vcy、λcRespectively representing the east speed, the north speed, the latitude and the longitude of the carrier calculated by inertial navigation; w is acx、wcy、wczRepresenting the torque applied to the gyroscope, wherein the parameter is a to-be-solved quantity;
Vry、Vrxrepresenting the north and east speeds given by a Doppler log, by the velocity v at random trackx、vyVelocity measurement error V of upper-superposed Doppler logry、VrxObtaining;
under the condition of internal horizontal damping working condition of inertial navigation work, K10, B, C, D, E, 0, inertial navigation at external levelUnder damped conditions, K1=1,A=B=1,C=D=E=0;
ua~ufIntermediate variables in the resolving process;
the equation set only consists of a first-order linear homogeneous differential equation and a first-order equation, and can be solved by means of a Runge-Kutta method; wherein λc、φcThe true longitude and latitude of the carrier are obtained, and the true longitude and latitude of the carrier are subtracted from the longitude and latitude of the random track, so that the inertial navigation positioning error can be obtained.
6. The gravity-assisted navigation track planning method according to claim 4, wherein: step (4) of the step 2 adopts a gravity feature abundance degree calculation method to calculate that the total length is X0Speed of V0The specific steps of the GL value of the random track comprise:
firstly, translating a real track;
any one real track generated by a certain random track is translated to east and west 0.5 nautical miles along the latitude line, and a bilinear interpolation method is utilized to obtain the gravity abnormal value sequences on the two tracks, wherein the gravity abnormal value sequences are respectively as follows:
any one real track generated by a certain random track is translated to the south and the north by 0.5 nautical miles along the meridian line, and the gravity abnormal value sequences on the two tracks can be obtained by utilizing a bilinear interpolation method, wherein the sequences are respectively as follows:
secondly, calculating the gravity abnormal gradient on the real track:
on the real track, the gradient sequence Tx in the longitude direction and the gradient sequence Ty in the latitude direction are shown as follows:
and thirdly, calculating GL of the random track:
for any random track, the number of real tracks generated by simulation through an inertial navigation positioning error solution method is 100, and the average value of the gradient variances in all the real tracks in the longitude direction can be defined as follows:
the average of the gradient variances in all true track latitudes can be defined as follows:
wherein, Txi、TyiRespectively is a longitude and latitude gradient sequence on the ith real track;
then the gravity anomaly feature richness GL of the corresponding random track can be expressed as:
by combining the gravity characteristic abundance calculation method and the inertial navigation positioning error calculation method, the total length X can be calculated0Speed of flight V0The GL value of the random track, the random track with the maximum GL is the length X0Speed of V0Alternative tracks under conditions;
and further modifying the random track length X and the track speed V, and screening out the random track with the maximum GL under any track length and any track speed condition according to the method to form an alternative track set.
7. A gravity assisted navigation trajectory planning method according to claim 3, characterized by: the gravity measurement error model used in step 3 is shown as follows:
in the formula, g represents a local absolute gravity value and can be obtained by combining the latitude of an alternative track with a WGS-84 ellipsoid model;representing the latitude of the alternative track; v. ofeEast-direction speed representing an alternative track; v. ofnA northbound speed representing the alternative track; omega represents the rotational angular velocity of the earth, R represents the radius of the earth, A1=978032.67714,B1=0.00193185138639,C1=-0.00669437999013,D1-0.3086, constant;
in the above formula, gcFor gravity measurement errors to be solved, the error sources are divided into three categories: h is the error of the potential measuring instrument, and the value of the error can be found from the operation manual of the potential measuring instrument; a isE、aN、ve、vnThe error of information given for inertial navigation, α and β are the misalignment angle of the gravity meter platform, for the latter two kinds of error sources, the basic equation of horizontal damping inertial navigation can be used to solve and calculate by combining the Runge-Kutta method, and all the error sources are substituted into the formula to obtain gcA numerical solution of (c); the standard deviation g of the measured gravity error during the matching periodc=std(gc)。
8. A gravity assisted navigation trajectory planning method according to claim 3, characterized by: the inertial navigation positioning error calculation method in step 3 calculates the inertial navigation positioning error divergence values during the matching period on all the alternative tracks under the parameters and working mode conditions set in step 1, as shown in the following formula:
in the above formula, the subscript t ∈ [ t1,t2],t1And t2Represents the start-stop time of the match;λctrepresenting the longitude and latitude on the real track of the carrier calculated by the inertial navigation positioning error calculation method in the step 2;λtrepresenting alternative track longitude and latitude; the divergence value of inertial navigation positioning error during matching is max (t)。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711414134.8A CN108225310B (en) | 2017-12-22 | 2017-12-22 | Gravity-assisted navigation track planning method |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711414134.8A CN108225310B (en) | 2017-12-22 | 2017-12-22 | Gravity-assisted navigation track planning method |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108225310A CN108225310A (en) | 2018-06-29 |
CN108225310B true CN108225310B (en) | 2020-08-25 |
Family
ID=62648578
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201711414134.8A Active CN108225310B (en) | 2017-12-22 | 2017-12-22 | Gravity-assisted navigation track planning method |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108225310B (en) |
Families Citing this family (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109269526B (en) * | 2018-07-16 | 2022-06-17 | 哈尔滨工程大学 | Rotary grid inertial navigation horizontal damping method based on damping network |
CN109141426B (en) * | 2018-08-10 | 2020-11-10 | 中国空间技术研究院 | Method for matching navigation adaptation area by underwater gravity |
CN109059964B (en) * | 2018-09-19 | 2021-07-23 | 中国船舶重工集团公司第七0七研究所 | Inertial navigation and gravity measurement double-calibration method based on gravity peak |
CN109085655B (en) * | 2018-09-19 | 2020-06-19 | 中国船舶重工集团公司第七0七研究所 | Underwater platform gravity measurement scheme and verification method |
CN110031000B (en) * | 2019-05-21 | 2021-05-18 | 北京理工大学 | Method for evaluating adaptability of gravity-assisted inertial navigation area |
CN110196050B (en) * | 2019-05-29 | 2022-11-18 | 哈尔滨工程大学 | Vertical height and speed measuring method of strapdown inertial navigation system |
CN111006694B (en) * | 2019-12-29 | 2022-03-18 | 北京理工大学 | Design method of long-endurance inertial navigation system track generator based on track planning |
US11268813B2 (en) | 2020-01-13 | 2022-03-08 | Honeywell International Inc. | Integrated inertial gravitational anomaly navigation system |
CN111473790B (en) * | 2020-04-26 | 2021-08-31 | 中国人民解放军61540部队 | Submarine navigation method and system of gravity beacon along track |
CN111426313B (en) * | 2020-04-26 | 2021-09-07 | 中国人民解放军61540部队 | Line matching and surface matching fusion navigation method and system based on gravity lighthouse |
CN111721300A (en) * | 2020-06-30 | 2020-09-29 | 清华大学 | Gravity beacon navigation method and system based on artificial intelligence algorithm |
CN112541046B (en) * | 2020-11-30 | 2022-09-06 | 中国电子科技集团公司第二十八研究所 | Co-occurrence target monitoring method based on time and space |
CN112729275A (en) * | 2021-01-08 | 2021-04-30 | 中国船舶重工集团公司第七0七研究所 | Satellite inversion chart gravity adaptation area selection method utilizing factor analysis |
CN113514059B (en) * | 2021-05-19 | 2024-02-13 | 北京理工大学 | Gravity-assisted inertial navigation system simulation platform |
CN113503871B (en) * | 2021-05-19 | 2024-04-16 | 北京理工大学 | Gravity matching method based on correlation filtering |
CN113587923B (en) * | 2021-05-31 | 2024-04-26 | 中国人民解放军61540部队 | Submersible positioning method and system for screening matching areas of multidimensional gravity gradient lighthouse |
CN114136320B (en) * | 2021-11-19 | 2023-06-20 | 中国船舶重工集团公司第七0七研究所 | Ocean multi-physical-field parameter characteristic positioning fusion method based on characteristic complementation characteristics |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102128625A (en) * | 2010-12-08 | 2011-07-20 | 北京航空航天大学 | Initial matching method for use in gravimetric map matching in gravity-aided inertial navigation system |
CN103604430A (en) * | 2013-11-06 | 2014-02-26 | 哈尔滨工程大学 | Marginalized cubature Kalman filter (CKF)-based gravity aided navigation method |
CN105043388A (en) * | 2015-06-29 | 2015-11-11 | 中国船舶重工集团公司第七0七研究所 | Vector search iterative matching method based on inertia/gravity matching integrated navigation |
CN105157703A (en) * | 2015-06-03 | 2015-12-16 | 北京理工大学 | Evaluation method of navigability of gravity-assisted inertial navigation adaptation zone |
CN107289973A (en) * | 2017-06-22 | 2017-10-24 | 北京理工大学 | A kind of gravitational field suitability determination methods in Gravity Matching navigation |
-
2017
- 2017-12-22 CN CN201711414134.8A patent/CN108225310B/en active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102128625A (en) * | 2010-12-08 | 2011-07-20 | 北京航空航天大学 | Initial matching method for use in gravimetric map matching in gravity-aided inertial navigation system |
CN103604430A (en) * | 2013-11-06 | 2014-02-26 | 哈尔滨工程大学 | Marginalized cubature Kalman filter (CKF)-based gravity aided navigation method |
CN105157703A (en) * | 2015-06-03 | 2015-12-16 | 北京理工大学 | Evaluation method of navigability of gravity-assisted inertial navigation adaptation zone |
CN105043388A (en) * | 2015-06-29 | 2015-11-11 | 中国船舶重工集团公司第七0七研究所 | Vector search iterative matching method based on inertia/gravity matching integrated navigation |
CN107289973A (en) * | 2017-06-22 | 2017-10-24 | 北京理工大学 | A kind of gravitational field suitability determination methods in Gravity Matching navigation |
Also Published As
Publication number | Publication date |
---|---|
CN108225310A (en) | 2018-06-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108225310B (en) | Gravity-assisted navigation track planning method | |
Jalving et al. | A toolbox of aiding techniques for the HUGIN AUV integrated inertial navigation system | |
Gade | NavLab, a generic simulation and post-processing tool for navigation | |
CN103743395B (en) | The compensation method of time delay in a kind of inertia-gravity coupling integrated navigation system | |
CN107990891B (en) | Underwater robot combined navigation method based on long baseline and beacon online calibration | |
CN105091907B (en) | DVL orientation alignment error method of estimation in SINS/DVL combinations | |
CN109855617A (en) | A kind of vehicle positioning method, vehicle locating device and terminal device | |
CN102829777A (en) | Integrated navigation system for autonomous underwater robot and method | |
CN104197927A (en) | Real-time navigation system and real-time navigation method for underwater structure detection robot | |
CN104807479A (en) | Inertial navigation alignment performance evaluation method based on main inertial navigation attitude variation quantity assistance | |
US20160153786A1 (en) | Devices, program products and computer implemented methods for touchless metrology having virtual zero-velocity and position update | |
CN111024074B (en) | Inertial navigation speed error determination method based on recursive least square parameter identification | |
CN103604430A (en) | Marginalized cubature Kalman filter (CKF)-based gravity aided navigation method | |
Liu et al. | SINS/DVL integrated system with current and misalignment estimation for midwater navigation | |
Stanway | Contributions to automated realtime underwater navigation | |
CN202928583U (en) | Offshore drilling platform attitude monitor and location device | |
CN102385060A (en) | Arrangement and method used for two-dimensional and three-dimensional exact position and directional determination | |
Morice et al. | Geometric bounding techniques for underwater localisation using range-only sensors | |
CN114111840B (en) | DVL error parameter online calibration method based on integrated navigation | |
CN115060275A (en) | Navigation information optimization method for multiple inertial navigation devices | |
Silva et al. | Optimizing the gains of baro-inertial vertical channel mechanizations | |
Binder | Construction of a geographically oriented horizon trihedron in gyroscopic orientation systems intended to aid navigation dead reckoning part 1. Gyroscopic orientation with a correctable pendulum. Implementation in a free gyroscope | |
Ai et al. | A Real-Time Autonomous Gravity Measurement Method for Underwater Gravity-Aided Navigation | |
US20130238237A1 (en) | Multiple data sources pedestrian navigation system | |
RU2723976C1 (en) | Method for determining angular orientation of ground vehicle |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |