CN113834502A - Initial alignment method and latitude calculation method for strapdown inertial navigation - Google Patents

Initial alignment method and latitude calculation method for strapdown inertial navigation Download PDF

Info

Publication number
CN113834502A
CN113834502A CN202111121776.5A CN202111121776A CN113834502A CN 113834502 A CN113834502 A CN 113834502A CN 202111121776 A CN202111121776 A CN 202111121776A CN 113834502 A CN113834502 A CN 113834502A
Authority
CN
China
Prior art keywords
coordinate system
inertial
time
current
navigation
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
Application number
CN202111121776.5A
Other languages
Chinese (zh)
Inventor
李万里
陈明剑
李军正
王力
赵远
张好
周舒涵
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Information Engineering University of PLA Strategic Support Force
Original Assignee
Information Engineering University of PLA Strategic Support Force
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Information Engineering University of PLA Strategic Support Force filed Critical Information Engineering University of PLA Strategic Support Force
Priority to CN202111121776.5A priority Critical patent/CN113834502A/en
Publication of CN113834502A publication Critical patent/CN113834502A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • 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)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

The invention belongs to the technical field of strapdown inertial navigation, and particularly relates to an initial alignment method and a latitude calculation method of strapdown inertial navigation. The invention firstly utilizes specific force information f under a carrier coordinate system b measured by an accelerometerbCalculating a first ratio integral value S1And a second specific force integral value S2Calculating an included angle theta between the gravity vectors by utilizing the two specific force integral values, and further calculating to obtain the current latitude L by combining alpha; the initial alignment may be performed using the current latitude. The method fully utilizes historical data to determine the current latitude, effectively eliminates the influence of base disturbance or carrier motion, realizes accurate estimation of the current latitude, and ensures the initial alignment precision.

Description

Initial alignment method and latitude calculation method for strapdown inertial navigation
Technical Field
The invention belongs to the technical field of strapdown inertial navigation, and particularly relates to an initial alignment method and a latitude calculation method of strapdown inertial navigation.
Background
Strapdown inertial navigation requires initial alignment to be completed before use to obtain initial pose information. Typically, the initial alignment is generally done with a known position (primarily latitude). However, in some special circumstances, such as: underwater, tunnel, or satellite navigation signals are disturbed, and the navigation system cannot acquire accurate position information. Therefore, the research on the initial alignment method under the condition of unknown latitude has certain theoretical significance and practical application value.
At present, an initial alignment method under the condition of unknown latitude mainly estimates an initial latitude by means of measurement values of a gyroscope and an accelerometer in strapdown inertial navigation, and then performs initial alignment by using the estimated latitude.
Under the condition of a static base, the output measurement values of the gyroscope and the accelerometer are respectively equal to the rotational angular velocity and the gravitational acceleration of the earth, and the estimation of the initial latitude and the initial alignment can be finished by utilizing the characteristic that the size of an included angle between two vectors is not changed due to the specific coordinate projection expression mode of the two vectors. Under the condition of a moving base, because the measurement values of the gyroscope and the accelerometer contain disturbance measurement values, an inertial system is generally adopted to align the frame, and the influence caused by disturbance acceleration is eliminated.
The conventional initial alignment method usually simply selects a few vectors to perform latitude calculation and initial alignment, and the latitude calculation accuracy is low, so that the initial alignment accuracy is also low.
Disclosure of Invention
The invention provides a latitude calculation method, which is used for solving the problem of low latitude calculation precision in the prior art; the invention also provides an initial alignment method of the strapdown inertial navigation, which is used for solving the problem of low initial alignment precision caused by low latitude calculation precision in the prior art.
In order to solve the technical problems, the technical scheme and the corresponding beneficial effects of the technical scheme are as follows:
the invention discloses a latitude computing method, which comprises the following steps:
a) in strapdown inertial navigationIn the process of guiding work, a carrier coordinate system is solidified according to the inertia of the current moment
Figure BDA0003277495910000011
And a direction cosine matrix between the carrier coordinate system b
Figure BDA0003277495910000012
And specific force information f under a carrier coordinate system b measured by an accelerometerbCalculating an inertia solidification carrier coordinate system from an initial time to a middle time
Figure BDA0003277495910000013
First specific force integral value S1And an inertial solidification carrier coordinate system from the intermediate time to the current time
Figure BDA0003277495910000014
Second specific force integral value S2(ii) a Wherein, the intermediate time is half of the current time;
b) according to the first ratio integral value S1Second specific force integral value S2And determining an inertial solidification carrier coordinate system according to the following formula
Figure BDA0003277495910000021
The included angle theta between the gravity vectors from the following initial time to the current time:
Figure BDA0003277495910000022
c) according to the angle alpha of the earth rotating from the initial time to the current time and the inertial solidification carrier coordinate system
Figure BDA0003277495910000023
And calculating the current latitude L according to an included angle theta between the gravity vectors from the initial moment to the current moment.
The beneficial effects of the above technical scheme are: the invention makes efficient use of historical data prior to the current time, including accelerationSpecific force information f under carrier coordinate system b obtained by measurement of a meterbUsing fbAnd after updating
Figure BDA0003277495910000024
Calculating to obtain a first ratio integral value S1And a second specific force integral value S2And further combining alpha and inertia solidification carrier coordinate systems
Figure BDA0003277495910000025
And calculating an included angle theta between the gravity vectors from the initial moment to the current moment to obtain the current latitude. The method fully utilizes historical data to determine the current latitude, effectively eliminates the influence of base disturbance or carrier motion, realizes accurate estimation of the current latitude, and ensures the initial alignment precision.
Further, in step a), the first ratio integral value S is calculated by the following method1And the second specific force integral value S2：
Coordinate system for calculating inertial solidification carrier in unit time
Figure BDA0003277495910000026
The integral value of the specific force;
setting the inertia in each unit time from the initial time to the middle time as a coordinate system of the solid carrier
Figure BDA0003277495910000027
Adding the lower specific force integral value to obtain the first specific force integral value S1Setting the coordinate system of the inertial solidification carrier in each unit time from the middle time to the current time
Figure BDA0003277495910000028
Adding the lower specific force integral value to obtain the second specific force integral value S2：
Figure BDA0003277495910000029
Figure BDA00032774959100000210
Figure BDA00032774959100000211
Wherein t is the current moment, and t/2 is the intermediate moment; v1(m) is an inertia solidification carrier coordinate system from the m-1 th moment to the m th moment
Figure BDA00032774959100000212
The integral value of the specific force, and the time contained from the m-1 st time point to the m-th time point is a unit time.
Further, in order to ensure the first ratio integral value S1And the second specific force integral value S2The accuracy of the calculation, step a), is such that the first ratio integral value S is calculated only if the current time is greater than a set time1And the second specific force integral value S2。
Further, in step a), the inertial solidification carrier coordinate system at the current moment
Figure BDA0003277495910000031
And a direction cosine matrix between the carrier coordinate system b
Figure BDA0003277495910000032
The following formula is adopted for updating:
Figure BDA0003277495910000033
wherein,
Figure BDA0003277495910000034
for gyroscope output values
Figure BDA0003277495910000035
Is determined by the skew-symmetric matrix of (a),
Figure BDA0003277495910000036
is a direction cosine matrix
Figure BDA0003277495910000037
The derivative of (c).
Further, in step c), the current latitude L is calculated by using the following formula:
Figure BDA0003277495910000038
further, in order to eliminate noise interference to reduce the influence of carrier motion or base disturbance, specific force information f under the acquired carrier coordinate system b is further includedbAnd performing low-pass filtering, wherein the adopted low-pass filter is an FIR filter or an IIR filter.
The invention discloses an initial alignment method of strapdown inertial navigation, which comprises the following steps:
A) determining a current latitude L by using the latitude calculation method according to any one of claims 1 to 6;
B) calculating the inertial coagulation navigation coordinate system of the current moment by using the determined current latitude L
Figure BDA0003277495910000039
Attitude transformation matrix to navigation coordinate system n
Figure BDA00032774959100000310
C) Navigation coordinate system using inertial coagulation
Figure BDA00032774959100000311
Attitude transformation matrix of navigation coordinate system n to current moment
Figure BDA00032774959100000312
Determining a posture conversion matrix from a carrier coordinate system b to a navigation coordinate system n at the current moment
Figure BDA00032774959100000313
Repeating steps A) through C) to complete the initial alignment.
The beneficial effects of the above technical scheme are: the invention firstly realizes the estimation of latitude information under the condition of unknown latitude. The specific means is as follows: historical data before the current moment is effectively utilized, and the historical data comprises specific force information f in a carrier coordinate system b measured by an accelerometerbUsing fbAnd after updating
Figure BDA00032774959100000314
Calculating to obtain a first ratio integral value S1And a second specific force integral value S2And further combining alpha and inertia solidification carrier coordinate systems
Figure BDA00032774959100000315
And calculating an included angle theta between the gravity vectors from the initial moment to the current moment to obtain the current latitude. Initial alignment is then performed using the estimated latitude information. The method fully utilizes historical data to determine the current latitude, effectively eliminates the influence of base disturbance or carrier motion, realizes accurate estimation of the current latitude, and ensures the initial alignment precision.
Further, the attitude transformation matrix is obtained for accurate calculation
Figure BDA0003277495910000041
In the step B), calculating an inertial coagulation navigation coordinate system by adopting the following formula
Figure BDA0003277495910000042
Attitude transformation matrix of navigation coordinate system n to current moment
Figure BDA0003277495910000043
Figure BDA0003277495910000044
Figure BDA0003277495910000045
Wherein, ω isieIs the rotational angular velocity of the earth.
Further, in order to fully utilize information of all inertial navigation systems to improve the accuracy of initial alignment, in step C), the attitude transformation matrix from the carrier coordinate system b to the navigation coordinate system n at the current moment is used as the attitude transformation matrix
Figure BDA0003277495910000046
Comprises the following steps:
Figure BDA0003277495910000047
wherein,
Figure BDA0003277495910000048
coordinate system for solidifying carrier by inertia
Figure BDA0003277495910000049
To inertial coagulation navigation coordinate system
Figure BDA00032774959100000410
A conversion matrix of, and
Figure BDA00032774959100000411
V1(t) is an inertia solidification carrier coordinate system from a certain moment to the current moment
Figure BDA00032774959100000412
Integral value of specific force of2(t) is an inertial coagulation navigation coordinate system from a certain moment to the current moment
Figure BDA00032774959100000413
The value of the following gravity integral, and:
Figure BDA00032774959100000414
wherein, t0Is a certain moment;
Figure BDA00032774959100000415
navigating a coordinate system for inertial coagulation
Figure BDA00032774959100000416
The following gravitational acceleration vector:
Figure BDA00032774959100000417
further, the transformation matrix is obtained for accurate calculation
Figure BDA00032774959100000418
The inertia solidification carrier coordinate system
Figure BDA00032774959100000419
To inertial coagulation navigation coordinate system
Figure BDA00032774959100000420
Is converted into a matrix
Figure BDA00032774959100000421
Using formulas
Figure BDA00032774959100000422
And solving by adopting a singular value solution method.
Drawings
FIG. 1 is a schematic view of the gravitational acceleration vector in the inertial frame of the present invention;
FIG. 2 is a flow chart of the strap-down inertial navigation initial alignment method of the present invention.
Detailed Description
The method can realize latitude estimation under the condition of unknown latitude and realize initial alignment of strapdown inertial navigation on the basis of latitude estimation.
The following first explains a specific implementation concept of latitude estimation:
selecting an east-north-sky geographic coordinate system as a navigation coordinate system and recording the geographic coordinate system as n; e is a terrestrial coordinate system; b is a vector coordinate system of 'right-front-upper'; i is an inertial coordinate system;
Figure BDA0003277495910000051
a coordinate system for inertially solidifying the carrier, which is at t0Obtaining a carrier coordinate system b through inertial solidification at any moment;
Figure BDA0003277495910000052
navigating a coordinate system for inertial coagulation, which is at t0And (4) obtaining the navigation coordinate system n through inertial solidification at any moment.
As shown in fig. 1, the motion trajectory of the gravity acceleration vector in the inertial coordinate system forms a conical surface. Latitude information can be estimated from the gravitational acceleration vector. Coordinate system of inertia solidification carrier
Figure BDA0003277495910000053
Lower t1Gravity vector of time of day
Figure BDA0003277495910000054
And t2Of time of day
Figure BDA0003277495910000055
The included angle between the two lines is marked as theta, and the line segments in the graph obtained from FIG. 1 have the following relationship:
Figure BDA0003277495910000056
Figure BDA0003277495910000057
|MO′|＝|MO|cosL (3)
wherein L is the current latitude, and alpha is t1Time t2The angle the earth is spinning at the moment. The combined type (1) to (3) can obtain:
Figure BDA0003277495910000058
the following can be obtained:
Figure BDA0003277495910000059
because:
α＝ωie(t2-t1) (6)
wherein, ω isieIs to self-calculate the angular velocity. The key to the problem is to solve for the angle theta, which can be determined from
Figure BDA00032774959100000510
And
Figure BDA00032774959100000511
and calculating to obtain:
Figure BDA00032774959100000512
under the condition of shaking the base, the method comprises the following steps:
Figure BDA00032774959100000513
wherein,
Figure BDA0003277495910000061
coordinate system for representing inertial solidification carrier
Figure BDA0003277495910000062
And the specific force information under the system is measured by an accelerometer. Then:
Figure BDA0003277495910000063
in order to improve the estimation accuracy, an integral value within a period of time is taken for smoothing, and then the included angle is calculated. Order:
Figure BDA0003277495910000064
Figure BDA0003277495910000065
taking:
Figure BDA0003277495910000066
wherein:
Figure BDA0003277495910000067
under the condition of disturbing the base, the specific force information f under the carrier coordinate system b measured by the accelerometerbThe method comprises interference measured values brought by base disturbance, vehicle-mounted disturbance acceleration is generally distributed in a region above 0.5Hz, carrier-based disturbance acceleration is generally distributed in a region above 0.02Hz, and the influence of the interference measured values can be reduced by designing a low-pass filter.
Figure BDA0003277495910000068
Coordinate system for solidifying carrier by inertia
Figure BDA0003277495910000069
And a direction cosine matrix between the vector coordinate system b, wherein the initial value of the direction cosine matrix is an identity matrix:
Figure BDA00032774959100000610
this matrix can be updated with gyro outputs:
Figure BDA00032774959100000611
wherein,
Figure BDA00032774959100000612
for gyroscope output values
Figure BDA00032774959100000613
Is determined by the skew-symmetric matrix of (a),
Figure BDA00032774959100000614
is a matrix
Figure BDA00032774959100000615
The derivative of (c).
Then:
Figure BDA00032774959100000616
and then simultaneous equations (5) and (6) can be solved to obtain the current latitude.
Then, the initial alignment (i.e. solving the attitude matrix from the carrier coordinate system b to the navigation coordinate system n at the current moment) is performed
Figure BDA00032774959100000617
) The implementation concept of (2) is explained:
the mechanical layout equation of the strapdown inertial navigation system under the geographic coordinate system is as follows:
Figure BDA0003277495910000071
wherein,
Figure BDA0003277495910000072
is the velocity derivative in the navigation coordinate system;
Figure BDA0003277495910000073
for the transformation moment of the terrestrial coordinate system e to the navigation coordinate system nArraying;
Figure BDA0003277495910000074
representing the rotation angular speed of the earth in a navigation coordinate system n system;
Figure BDA0003277495910000075
representing the rotation angular speed of the earth coordinate system e relative to the navigation coordinate system n in the navigation coordinate system n; v. ofnThe speed under the navigation coordinate system n system; gnIs the representation of the gravity acceleration in a navigation coordinate system n system; f. ofbIs specific force information under a carrier coordinate system b.
Under turbulent conditions, velocity vn≈0、
Figure BDA0003277495910000076
Equation (17) can be simplified as:
Figure BDA0003277495910000077
according to the chain rule, the attitude matrix of the strapdown inertial navigation can be decomposed into:
Figure BDA0003277495910000078
wherein,
Figure BDA0003277495910000079
navigating a coordinate system for inertial coagulation
Figure BDA00032774959100000710
A transformation matrix to a navigation coordinate system n;
Figure BDA00032774959100000711
coordinate system for solidifying carrier by inertia
Figure BDA00032774959100000712
To inertial coagulation navigation coordinate system
Figure BDA00032774959100000713
The transformation matrix of (2);
Figure BDA00032774959100000714
from a carrier coordinate system b to an inertia solidification carrier coordinate system
Figure BDA00032774959100000715
The transformation matrix of (2).
Multiplying the two sides of the formula (18) by a navigation coordinate system n to an inertial coagulation navigation coordinate system
Figure BDA00032774959100000716
Is converted into a matrix
Figure BDA00032774959100000717
The following can be obtained:
Figure BDA00032774959100000718
wherein,
Figure BDA00032774959100000719
navigating a coordinate system for inertial coagulation
Figure BDA00032774959100000720
The lower gravitational acceleration vector.
Wherein,
Figure BDA00032774959100000721
and is
Figure BDA00032774959100000722
The updating can be performed according to the estimated current latitude:
Figure BDA00032774959100000723
then there are:
Figure BDA00032774959100000724
Figure BDA00032774959100000725
can be updated according to equation (15), thus, according to equation (19), only the matrix needs to be solved
Figure BDA00032774959100000726
The current attitude matrix can be obtained
Figure BDA00032774959100000727
Thereby completing the initial alignment.
In order to reduce the influence caused by the disturbance of the base, the measured value f of the specific force is contrasted in an integral modebSmoothing is performed. Defining:
Figure BDA0003277495910000081
Figure BDA0003277495910000082
and has the following components:
Figure BDA0003277495910000083
two three-dimensional point arrays { V ] can be obtained by selecting a certain integration interval1(i) And { V }2(i) 1,2,3 … N. By simply obtaining a transformation matrix between them
Figure BDA0003277495910000084
Then, the current attitude matrix can be obtained according to the formula (19). Thus, the alignment problem translates into the Wahba problem often encountered in engineering.
The invention solves the conversion matrix by using a singular value decomposition method
Figure BDA0003277495910000085
The solution of the transformation matrix is as follows:
Figure BDA0003277495910000086
let UDGT，D＝diag(di),d1≥d2≥d3A singular value boundary of M > 0, where U and G are both unitary matrices:
UUT＝GGT＝I3×3 (27)
and:
Figure BDA0003277495910000087
the optimal conversion parameter can be uniquely determined by the following formula:
A＝USV' (29)
Figure BDA0003277495910000088
after the principles of the two aspects are introduced, the following describes a strapdown inertial navigation initial alignment method and a latitude calculation method in detail with reference to the embodiments.
The embodiment of the strap-down inertial navigation initial alignment method comprises the following steps:
the flow of the embodiment of the strap-down inertial navigation initial alignment method is shown in fig. 2, and the process is as follows:
step one, an inertial navigation system is installed on a carrier according to an installation reference, and the system is electrified and started.
Step two, selecting a low-pass filter (optionally an FIR filter or an IIR filter) with the cutoff frequency of 0.5Hz to measure the specific force information f under the carrier coordinate system b obtained by the measurement of the accelerometerbAnd (6) filtering.
Step three, updating inertial coagulation according to a formula (15)Carrier coordinate system
Figure BDA0003277495910000091
And a direction cosine matrix between the carrier coordinate system b
Figure BDA0003277495910000092
And calculating an inertia solidification carrier coordinate system in each second at each integral second moment
Figure BDA0003277495910000093
Integral value of specific force V1(m) and storing. Wherein, define V1(m) is an inertia solidification carrier coordinate system from the m-1 th moment to the m th moment
Figure BDA0003277495910000094
The following specific force integral values:
Figure BDA0003277495910000095
step four, judging whether the alignment time is more than or equal to 20 seconds: if the time is less than 20 seconds, returning to the step two; if the time is more than or equal to 20 seconds, calculating the current latitude by using the following formula:
1) calculating an inertia solidification carrier coordinate system from the initial time to the middle time in each even number second
Figure BDA0003277495910000096
First specific force integral value S1And an inertial solidification carrier coordinate system from the intermediate time to the current time
Figure BDA0003277495910000097
Second specific force integral value S2Respectively as follows:
Figure BDA0003277495910000098
Figure BDA0003277495910000099
wherein t is the current time, and t/2 is the intermediate time.
2) According to equation (16), there are:
Figure BDA00032774959100000910
3) the current latitude L is updated according to the formula (5) and the formula (6). The estimated latitude effectively utilizes all gyro and accelerometer data before the current moment, and can effectively smooth the influence caused by base disturbance.
4) After the current latitude L is obtained, calculating the inertial coagulation navigation coordinate system in each integral second according to a formula (22)
Figure BDA00032774959100000911
Integral value of gravity V2(m) and storing. Wherein, define V2(m) is the inertial coagulation navigation coordinate system from the m-1 th time to the m th time
Figure BDA00032774959100000912
The following gravity integral values:
Figure BDA00032774959100000913
wherein,
Figure BDA00032774959100000914
see equation (22).
Step five, using two three-dimensional point arrays { V ] recorded in step three and step four every integer seconds1(i) And { V }2(i) Updating the inertial solid carrier coordinate system (i is 1,2,3 … t, t is the current time) and equations (26) - (30)
Figure BDA00032774959100000915
Navigation seat for inertial coagulationMarker system
Figure BDA0003277495910000101
Is converted into a matrix
Figure BDA0003277495910000102
The two point columns contain all historical data, and the influence caused by base disturbance can be effectively smoothed. Then using formula (21) to obtain the inertial coagulation navigation coordinate system
Figure BDA0003277495910000103
Attitude transformation matrix to navigation coordinate system n
Figure BDA0003277495910000104
Updating, and combining the updated inertial solidification carrier coordinate system
Figure BDA0003277495910000105
And a direction cosine matrix between the carrier coordinate system b
Figure BDA0003277495910000106
Using equation (19), the attitude matrix is completed
Figure BDA0003277495910000107
And (4) updating.
And step six, repeating the step two to the step five until the initial alignment is finished.
In summary, the initial alignment method of strapdown inertial navigation of the present invention has the following characteristics:
1. latitude information can be estimated and initial alignment can be accomplished with unknown latitude.
2. During latitude estimation, the influence caused by disturbance or carrier motion can be effectively reduced by using the low-pass filtering in the step two and the integration mode in the step three, and the latitude estimation precision is improved.
3. The initial alignment utilizes all specific force and angular velocity information of the inertial navigation system before the current moment, can effectively improve the accuracy of the initial alignment, and can effectively reduce the influence of disturbance on the accuracy of the initial alignment due to the adoption of low-pass filtering (aiming at noise reduction), integral smoothing and other modes.
4. According to the invention, during initial alignment, the information of all inertial navigation systems is effectively utilized, and the accuracy of initial alignment is effectively improved.
5. The whole method is convenient to implement and easy to execute on the machine.
In this embodiment, in order to calculate the local latitude L, the formula is
Figure BDA0003277495910000108
Because of the corresponding relation between sine and cosine, the formula can be used
Figure BDA0003277495910000109
And calculating to obtain the local latitude L. But the essence is obtained by using trigonometric functional relations between theta, alpha and L.
In the present embodiment, two three-dimensional dot arrays { V }1(i) And { V }2(i) V in1(i)、V2(i) Are calculated and stored in units of seconds. When the demand for calculation accuracy is higher, calculation may be performed in 1/2 seconds and stored, and when the demand for calculation accuracy is not so high but calculation efficiency is higher, calculation may be performed in 2 seconds, 3 seconds, or even larger.
The embodiment of the latitude calculation method comprises the following steps:
the embodiment of the latitude calculation method of the invention has the following overall thought:
firstly, in the working process of strapdown inertial navigation, a carrier coordinate system is solidified according to the inertia of the current moment
Figure BDA00032774959100001010
And a direction cosine matrix between the carrier coordinate system b
Figure BDA0003277495910000111
And specific force information f under a carrier coordinate system b measured by an accelerometerbAccording to the inertia from the initial time to the intermediate timeSex coagulation carrier coordinate system
Figure BDA0003277495910000112
First specific force integral value S1And an inertial solidification carrier coordinate system from the intermediate time to the current time
Figure BDA0003277495910000113
Second specific force integral value S2(ii) a Wherein, the intermediate time is half of the current time;
then, an inertial solidification carrier coordinate system is determined according to the following formula
Figure BDA0003277495910000114
The included angle theta between the gravity vectors from the following initial time to the current time:
Figure BDA0003277495910000115
finally, the carrier coordinate system is solidified according to alpha and inertia
Figure BDA0003277495910000116
And calculating the current latitude L according to an included angle theta between the gravity vectors from the initial moment to the current moment.
The method can effectively reduce the influence caused by disturbance, improve the latitude estimation precision and further effectively improve the precision of initial alignment. The whole method flow includes steps one to four in the embodiment of the strapdown inertial navigation initial alignment method, which is not described herein again.

Claims (10)

1. A latitude calculation method is characterized by comprising the following steps:
a) in the working process of strapdown inertial navigation, a carrier coordinate system is solidified according to the inertia of the current moment
Figure FDA0003277495900000011
And a direction cosine matrix between the carrier coordinate system b
Figure FDA0003277495900000012
And specific force information f under a carrier coordinate system b measured by an accelerometerbCalculating an inertia solidification carrier coordinate system from an initial time to a middle time
Figure FDA0003277495900000013
First specific force integral value S1And an inertial solidification carrier coordinate system from the intermediate time to the current time
Figure FDA0003277495900000014
Second specific force integral value S2(ii) a Wherein, the intermediate time is half of the current time;
b) according to the first ratio integral value S1Second specific force integral value S2And determining an inertial solidification carrier coordinate system according to the following formula
Figure FDA0003277495900000015
The included angle theta between the gravity vectors from the following initial time to the current time:
Figure FDA0003277495900000016
c) according to the angle alpha of the earth rotating from the initial time to the current time and the inertial solidification carrier coordinate system
Figure FDA0003277495900000017
And calculating the current latitude L according to an included angle theta between the gravity vectors from the initial moment to the current moment.
2. The latitude calculation method according to claim 1, wherein in step a), the first ratio integral value S is calculated by the following method1And the second specific force integral value S2：
When calculating the unit timeInternally inertial coagulation carrier coordinate system
Figure FDA0003277495900000018
The integral value of the specific force;
setting the inertia in each unit time from the initial time to the middle time as a coordinate system of the solid carrier
Figure FDA0003277495900000019
Adding the lower specific force integral value to obtain the first specific force integral value S1Setting the coordinate system of the inertial solidification carrier in each unit time from the middle time to the current time
Figure FDA00032774959000000110
Adding the lower specific force integral value to obtain the second specific force integral value S2：
Figure FDA00032774959000000111
Figure FDA00032774959000000112
Figure FDA00032774959000000113
Wherein t is the current moment, and t/2 is the intermediate moment; v1(m) is an inertia solidification carrier coordinate system from the m-1 th moment to the m th moment
Figure FDA00032774959000000114
The integral value of the specific force, and the time contained from the m-1 st time point to the m-th time point is a unit time.
3. The latitude calculation method according to claim 1, wherein in step a), only when the current time is greater than the set timeIn the case of a moment, the first ratio integral value S is calculated1And the second specific force integral value S2。
4. The latitude calculation method according to claim 1, wherein in step a), the inertial condensed carrier coordinate system at the current time is used
Figure FDA0003277495900000021
And a direction cosine matrix between the carrier coordinate system b
Figure FDA0003277495900000022
The following formula is adopted for updating:
Figure FDA0003277495900000023
wherein,
Figure FDA0003277495900000024
for gyroscope output values
Figure FDA0003277495900000025
Is determined by the skew-symmetric matrix of (a),
Figure FDA0003277495900000026
is a direction cosine matrix
Figure FDA0003277495900000027
The derivative of (c).
5. The latitude calculation method according to claim 1, wherein in step c), the current latitude L is calculated using the following formula:
Figure FDA0003277495900000028
6. the latitude calculation method according to claim 1, further comprising obtaining specific force information f in the carrier coordinate system bbAnd performing low-pass filtering, wherein the adopted low-pass filter is an FIR filter or an IIR filter.
7. A strap-down inertial navigation initial alignment method is characterized by comprising the following steps:
A) determining a current latitude L by using the latitude calculation method according to any one of claims 1 to 6;
B) calculating the inertial coagulation navigation coordinate system of the current moment by using the determined current latitude L
Figure FDA0003277495900000029
Attitude transformation matrix to navigation coordinate system n
Figure FDA00032774959000000210
C) Navigation coordinate system utilizing inertial coagulation at current moment
Figure FDA00032774959000000211
Attitude transformation matrix to navigation coordinate system n
Figure FDA00032774959000000212
Determining a posture conversion matrix from a carrier coordinate system b to a navigation coordinate system n at the current moment
Figure FDA00032774959000000213
Repeating steps A) through C) to complete the initial alignment.
8. The strap-down inertial navigation initial alignment method according to claim 7, wherein in step B), the inertial coagulation navigation coordinate system at the current time is calculated by using the following formula
Figure FDA00032774959000000214
Attitude transformation matrix to navigation coordinate system n
Figure FDA00032774959000000215
Figure FDA00032774959000000216
Wherein, ω isieIs the rotational angular velocity of the earth.
9. The strap-down inertial navigation initial alignment method according to claim 7, wherein in step C), the attitude transformation matrix of the carrier coordinate system b to the navigation coordinate system n at the current time is converted into the attitude transformation matrix of the navigation coordinate system n at the current time
Figure FDA0003277495900000031
Comprises the following steps:
Figure FDA0003277495900000032
wherein,
Figure FDA0003277495900000033
coordinate system for solidifying carrier by inertia
Figure FDA0003277495900000034
To inertial coagulation navigation coordinate system
Figure FDA0003277495900000035
A conversion matrix of, and
Figure FDA0003277495900000036
V1(t) is an inertia solidification carrier coordinate system from a certain moment to the current moment
Figure FDA0003277495900000037
Integral value of specific force of2(t) is an inertial coagulation navigation coordinate system from a certain moment to the current moment
Figure FDA0003277495900000038
The value of the following gravity integral, and:
Figure FDA0003277495900000039
wherein, t0Is a certain moment;
Figure FDA00032774959000000310
navigating a coordinate system for inertial coagulation
Figure FDA00032774959000000311
The following gravitational acceleration vector:
Figure FDA00032774959000000312
10. the strapdown inertial navigation initial alignment method of claim 9, wherein the inertial coagulation carrier coordinate system
Figure FDA00032774959000000313
To inertial coagulation navigation coordinate system
Figure FDA00032774959000000314
Is converted into a matrix
Figure FDA00032774959000000315
Using formulas
Figure FDA00032774959000000316
And solving by adopting a singular value solution method.
CN202111121776.5A 2021-09-24 2021-09-24 Initial alignment method and latitude calculation method for strapdown inertial navigation Pending CN113834502A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111121776.5A CN113834502A (en) 2021-09-24 2021-09-24 Initial alignment method and latitude calculation method for strapdown inertial navigation

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111121776.5A CN113834502A (en) 2021-09-24 2021-09-24 Initial alignment method and latitude calculation method for strapdown inertial navigation

Publications (1)

Publication Number Publication Date
CN113834502A true CN113834502A (en) 2021-12-24

Family

ID=78969852

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111121776.5A Pending CN113834502A (en) 2021-09-24 2021-09-24 Initial alignment method and latitude calculation method for strapdown inertial navigation

Country Status (1)

Country Link
CN (1) CN113834502A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023202262A1 (en) * 2022-04-19 2023-10-26 千寻位置网络有限公司 Inertial navigation initial-alignment method and apparatus applied to oblique measurement, and device

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105318872A (en) * 2014-07-31 2016-02-10 精工爱普生株式会社 Position calculation method and position calculation device
CN106123921A (en) * 2016-07-10 2016-11-16 北京工业大学 Latitude the unknown Alignment Method of SINS under the conditions of dynamic disturbance

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105318872A (en) * 2014-07-31 2016-02-10 精工爱普生株式会社 Position calculation method and position calculation device
CN106123921A (en) * 2016-07-10 2016-11-16 北京工业大学 Latitude the unknown Alignment Method of SINS under the conditions of dynamic disturbance

Non-Patent Citations (7)

* Cited by examiner, † Cited by third party
Title
严恭敏;严卫生;徐德民;蒋浩;: "纬度未知条件下捷联惯导系统初始对准分析", 航天控制, no. 02 *
严恭敏等: "纬度未知条件下的抗扰动惯性系初始对准改进方法", 《中国惯性技术学报》, vol. 28, no. 02, pages 141 - 146 *
张祺: "用于动力电池整包快速检测的容量估算方法研究", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》, no. 03, pages 035 - 362 *
王跃钢;杨家胜;杨波;: "纬度未知条件下捷联惯导系统晃动基座的初始对准", 航空学报, no. 12 *
苏忆: "双通道式车辆事故自动呼救系统的触发算法设计", 《机械设计与制造》, no. 02, pages 71 - 74 *
葛磊等: "一种改进的晃动基座上纬度自估计方法", 《导航与控制》, vol. 16, no. 2, pages 109 - 113 *
赵政等: "一种提高捷联惯导系统动基座初始对准精度的方法", 《导航与控制》, vol. 17, no. 5, pages 66 - 71 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023202262A1 (en) * 2022-04-19 2023-10-26 千寻位置网络有限公司 Inertial navigation initial-alignment method and apparatus applied to oblique measurement, and device

Similar Documents

Publication Publication Date Title
CN101949703B (en) Strapdown inertial/satellite combined navigation filtering method
CN103743395B (en) The compensation method of time delay in a kind of inertia-gravity coupling integrated navigation system
CN106595652B (en) Alignment methods between the backtracking formula of vehicle kinematics constraint auxiliary is advanced
CN110631574B (en) inertia/odometer/RTK multi-information fusion method
CN113405563B (en) Inertial measurement unit alignment method
CN112629538A (en) Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering
CN101187567A (en) Optical fiber gyroscope strap-down inertial navigation system initial posture determination method
CN109870173A (en) A kind of track correct method of the submarine pipeline inertial navigation system based on checkpoint
CN105180937A (en) Initial alignment method for MEMS-IMU
CN109945895B (en) Inertial navigation initial alignment method based on fading smooth variable structure filtering
CN111722295B (en) Underwater strapdown gravity measurement data processing method
CN103148868B (en) Based on the combination alignment methods that Doppler log Department of Geography range rate error is estimated under at the uniform velocity sailing through to
CN109959374B (en) Full-time and full-range reverse smooth filtering method for pedestrian inertial navigation
CN107677292B (en) Vertical line deviation compensation method based on gravity field model
CN113340298B (en) Inertial navigation and dual-antenna GNSS external parameter calibration method
CN105606093B (en) Inertial navigation method and device based on gravity real-Time Compensation
CN113959462A (en) Quaternion-based inertial navigation system self-alignment method
CN117053802A (en) Method for reducing positioning error of vehicle navigation system based on rotary MEMS IMU
CN113834502A (en) Initial alignment method and latitude calculation method for strapdown inertial navigation
CN111912427B (en) Method and system for aligning motion base of strapdown inertial navigation assisted by Doppler radar
CN113108781B (en) Improved coarse alignment method applied to unmanned ship during advancing
CN113008229B (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN110873577B (en) Underwater rapid-acting base alignment method and device
CN105300407A (en) Marine dynamic starting method for uniaxial modulation laser gyro inertial navigation system
CN109945857B (en) Vehicle-mounted inertial positioning method and device for real estate field measurement

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