CN104634348A - Attitude angle calculation method in integrated navigation - Google Patents
Attitude angle calculation method in integrated navigation Download PDFInfo
- Publication number
- CN104634348A CN104634348A CN201510109100.2A CN201510109100A CN104634348A CN 104634348 A CN104634348 A CN 104634348A CN 201510109100 A CN201510109100 A CN 201510109100A CN 104634348 A CN104634348 A CN 104634348A
- Authority
- CN
- China
- Prior art keywords
- mrow
- msubsup
- msub
- angle
- omega
- 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.)
- Granted
Links
- 238000004364 calculation method Methods 0.000 title claims abstract description 26
- 239000011159 matrix material Substances 0.000 claims abstract description 87
- 238000013016 damping Methods 0.000 claims abstract description 12
- 238000000034 method Methods 0.000 claims description 31
- 238000005096 rolling process Methods 0.000 claims description 30
- 239000013598 vector Substances 0.000 claims description 24
- 230000001133 acceleration Effects 0.000 claims description 23
- 230000008859 change Effects 0.000 claims description 13
- 238000001914 filtration Methods 0.000 claims description 13
- 238000012546 transfer Methods 0.000 claims description 10
- 230000002068 genetic effect Effects 0.000 claims description 6
- 238000000354 decomposition reaction Methods 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 238000012937 correction Methods 0.000 abstract description 14
- 238000005516 engineering process Methods 0.000 description 8
- 230000033001 locomotion Effects 0.000 description 8
- 238000005259 measurement Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 5
- 230000007547 defect Effects 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 238000002921 genetic algorithm search Methods 0.000 description 3
- 239000004576 sand Substances 0.000 description 3
- 230000009125 negative feedback regulation Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 241000287196 Asthenes Species 0.000 description 1
- 241000543540 Guillardia theta Species 0.000 description 1
- 241000592380 Nabis <genus> Species 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 239000005433 ionosphere Substances 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 239000005436 troposphere Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/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 provides an attitude angle calculation method in integrated navigation, and belongs to the field of the design of navigation. The attitude angle calculation method in the integrated navigation comprises the steps of establishing a closed-loop feedback circuit by adopting a preliminarily-acquired error angle and a preliminarily-acquired angular speed of a strapdown inertial navitation system as input and a follow-up angle as output in a follow-up angle correction way according to an incidence relation of the follow-up angle and a gain, a damping ratio and a natural angle frequency of the strapdown inertial navigation system circuit, further acquiring a follow-up angle with convergence characteristics to be used as an optimized follow-up angle, correcting an original strapdown matrix according to the optimized follow-up angle, determining a first optimized strapdown matrix, and acquiring an attitude angle according to the first optimized strapdown matrix, so that the precision calculation of the attitude angle is completed, and the weaknesses in the prior art can be solved.
Description
Technical Field
The invention relates to the field of navigation, in particular to a method for calculating an attitude angle in integrated navigation.
Background
The use of maps for marching or tourism exploration has been a routine practice. The purpose of using a map is to move toward the position of an object, and navigation using a map is also the most primitive navigation method. The basis for navigation is to obtain two elements, the first is the position of the target, and the second is to obtain the position of the target. With the progress of communication technology and information processing technology, two new navigation modes, namely inertial navigation technology and space satellite navigation technology, appear, and the two modes also aim at accurately acquiring the position of the navigation device.
The inertial navigation technology is a top technical subject in modern science and technology, and is widely applied to military fields of aviation, aerospace and navigation and many civil fields.
The strapdown inertial navigation system in the inertial navigation has the advantages of simple structure, small volume, light weight, low cost and high reliability. The navigation system is an autonomous navigation system in the true sense, continuously outputs information, autonomously provides information such as acceleration, speed, position, angular velocity, attitude angle and the like, completely depends on self equipment to autonomously complete navigation tasks, and does not have any photoelectric connection with the outside. Therefore, the novel solar energy mobile phone has the advantages of being good in concealment, free of external interference, free of limitation of time, region and weather conditions and the like. But navigation errors of the strapdown inertial navigation system increase as time accumulates.
Specifically, navigation is performed according to inertia, and the inertial element (accelerometer) is mainly used for measuring the acceleration of the vehicle, and the velocity and the position are obtained through integration and calculation, so that the purpose of navigation and positioning of the vehicle is achieved. The equipment forming the inertial navigation system is arranged in the carrier, does not depend on external information during working, does not radiate energy to the outside, is not easy to be interfered, and is an autonomous navigation system. Inertial navigation systems are typically comprised of an inertial measurement unit, a computer, a control display, and the like. The inertial measurement unit includes an accelerometer and a gyroscope, also called an inertial measurement unit. 3 single degree-of-freedom gyroscopes are used to measure 3 rotational movements of the carrier; 3 accelerometers are used to measure the acceleration of 3 translational movements of the vehicle. The computer calculates the speed and position data of the vehicle from the measured acceleration signals. And controlling the display to display various navigation parameters. According to the installation mode of the gyroscope and the accelerometer on the carrier, the inertial navigation system is divided into a platform type inertial navigation system (the gyroscope and the accelerometer are installed on a platform body of an inertial platform) and a strapdown type inertial navigation system (the gyroscope and the accelerometer are directly installed on the carrier).
In addition, the advent of satellite positioning systems has revolutionized the development of spatial navigation positioning technology. The satellite receiver has higher navigation positioning accuracy and is not reduced along with the increase of time, and the defects of the satellite receiver are that the data refresh rate is low, the satellite receiver is easy to be interfered, and the satellite is easy to lose to cause positioning interruption under a dynamic environment or under the condition of being shielded. Taking the GPS satellite navigation in the united states as an example to describe the positioning in the space navigation, the GPS satellite navigation system in the united states has 24 GPS satellites in total, and the 24 satellites are all located at 2-ten-thousand-2 hundred kilometers high above the ground, and each satellite runs around the earth in a period of 12 hours, so that at any time, more than 4 satellites can be observed at any point on the ground at the same time.
Since the positions of the satellites are known accurately, in the GPS observation, the distances from the satellites to the receiver can be obtained, 3 equations can be formed by using a distance formula in three-dimensional coordinates and 3 satellites, and the positions of the observation points are solved (X, Y and Z). Considering the error between the satellite clock and the receiver clock, there are actually 4 unknowns, X, Y, Z and clock difference, so that the 4 th satellite needs to be introduced to form 4 equations for solving, thereby obtaining the longitude and latitude and the elevation of the observation point.
The receiver can often lock more than 4 satellites, at this time, the receiver can be divided into a plurality of groups according to the constellation distribution of the satellites, each group has 4 satellites, and then one group with the minimum error is selected through an algorithm to be used for positioning, so that the precision is improved.
Due to errors of a satellite operation orbit and a satellite clock, the influence of atmosphere on a troposphere and an ionosphere on signals and a man-made SA protection policy, the positioning accuracy of the civil GPS is low. In order to improve the positioning accuracy, a differential GPS (dgps) technology is generally adopted, a reference station (differential station) is established to perform GPS observation, and known accurate coordinates of the reference station are used to compare with an observed value, so as to obtain a correction number, and the correction number is issued to the outside. After receiving the correction number, the receiver compares the correction number with the observed value of the receiver to eliminate most errors and obtain a more accurate position. Experiments show that the positioning accuracy can be improved by one order of magnitude by using the differential GPS.
The former two navigation modes have certain defects when confirming the position of the navigation mode, so a new navigation mode, namely combined navigation, is generated. The combined navigation algorithm optimally fuses signals of a strapdown inertial navigation system and a satellite receiver in a Kalman filtering mode, makes up for deficiencies, and makes up for the defects that dead zones exist in the signals of the satellite receiver and inertial navigation errors increase along with time, so that the combined system can ensure the precision and improve the reliability and the anti-interference capability. In the fusion method, the speed and the position are used as observed quantities, and an error value is estimated by using Kalman filtering recursion according to a system error state equation.
In the traditional algorithm, when the pitch angle of a motion carrier (self) adopting a combined navigation algorithm is very large, a mathematical platform calculated by a strapdown inertial navigation system is close to lose one degree of freedom, so that the calculation errors of navigation angles such as a course angle, a pitch angle and a roll angle are very large, and the speed error is directly very large. The Kalman filtering in the integrated navigation algorithm requires an accurate state equation model, and the model error caused by large errors of a course angle, a roll angle and a speed causes the deviation between the recursion state estimation and the real state of the Kalman filtering to be larger and larger, so that the performance of the integrated navigation system is rapidly deteriorated.
In conclusion, when the pitch angle of the moving carrier is large, the accuracy of the attitude angle is reduced, and further the accuracy of the conventional navigation mode is reduced.
Disclosure of Invention
In view of this, an object of the embodiments of the present invention is to provide a method for calculating an attitude angle in integrated navigation, so as to improve accuracy of the attitude angle and further improve accuracy of the integrated navigation.
In a first aspect, an embodiment of the present invention provides a method for calculating an attitude angle in integrated navigation, including:
taking a pre-acquired strapdown inertial navigation system error angle and angular velocity as input, taking a follow-up angle as output, and establishing a closed loop feedback loop according to the incidence relation between the follow-up angle and gain, damping ratio and natural angular frequency of a strapdown inertial navigation system loop, wherein the error angle is the difference value between a pitch angle and a preset critical angle;
calculating an optimized follow-up angle under a preset state according to the transfer function formula of the closed loop feedback loop;
correcting the pre-acquired original strapdown matrix by using the optimized follow-up angle to determine a first optimized strapdown matrix;
and acquiring an attitude angle according to the first optimized strapdown matrix.
With reference to the first aspect, an embodiment of the present invention provides a first possible implementation manner of the first aspect, where the method further includes:
and correcting the strapdown matrix of the current strapdown inertial navigation system according to the error compensation angle of the course angle and the error compensation angle of the rolling angle which are acquired in advance to determine the original strapdown matrix.
With reference to the first aspect, an embodiment of the present invention provides a second possible implementation manner of the first aspect, where the calculating an optimized follow-up angle in a predetermined state according to the transfer function equation of the closed-loop feedback loop includes:
discretizing the transfer function formula of the closed loop feedback loop according to a pre-acquired sampling period to determine a recursion formula for optimizing a follow-up angle;
and calculating the optimized follow-up angle according to the recursion formula of the optimized follow-up angle.
With reference to the first aspect, an embodiment of the present invention provides a third possible implementation manner of the first aspect, where the recurrence formula is Δ θ (k) ═ Δ θ1(k)+Δθ2(k),Δθ1(k)=a1Δθ1(k-1)+a2Δθ1(k-2)+b0θ(k)+b1θ(k-1)+b2θ(k-2),Δθ2(k)=c1Δθ2(k-1)+c2Δθ2(k-2)+c3Δθ2(k-3)+d0ωx(k)+d1ωx(k-1)+d2ωx(k-2)+d3ωx(k-3) wherein, <math>
<mrow>
<msub>
<mi>a</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mn>8</mn>
<mo>+</mo>
<msubsup>
<mrow>
<mn>2</mn>
<mi>Kω</mi>
</mrow>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>a</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mo>-</mo>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>-</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>b</mi>
<mn>0</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>θ</mi>
</mrow>
<mi>c</mi>
</msub>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
</mrow>
</math> <math>
<mrow>
<msub>
<mi>b</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mrow>
<mn>2</mn>
<mi>ω</mi>
</mrow>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>b</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mn>12</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mrow>
<mn>6</mn>
<mi>Kω</mi>
</mrow>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
</mrow>
</math> <math>
<mrow>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mo>-</mo>
<mn>12</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>-</mo>
<msubsup>
<mrow>
<mn>3</mn>
<mi>Kω</mi>
</mrow>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>c</mi>
<mn>3</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mn>4</mn>
<mo>-</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>d</mi>
<mn>0</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>3</mn>
</msubsup>
</mrow>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
<mo>,</mo>
</mrow>
</math> <math>
<mrow>
<msub>
<mi>d</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mrow>
<mn>3</mn>
<mi>ω</mi>
</mrow>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>3</mn>
</msubsup>
</mrow>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>d</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mrow>
<mn>3</mn>
<mi>ω</mi>
</mrow>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>3</mn>
</msubsup>
</mrow>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>d</mi>
<mn>3</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>3</mn>
</msubsup>
</mrow>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<mi>K</mi>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
<mo>,</mo>
</mrow>
</math> k is the gain of the loop of the strapdown inertial navigation system, zeta is the damping ratio of the loop of the strapdown inertial navigation system, omeganIs the natural angular frequency of the loop of the strapdown inertial navigation system.
With reference to the first aspect, an embodiment of the present invention provides a fourth possible implementation manner of the first aspect, where the method further includes:
calculating a reference pitch angle according to the following formula, theta ═ alpha thetas+(1-α)θaWherein, thetasFor a first reference pitch angle, theta, calculated by quaternionaA second reference pitch angle is calculated according to an acceleration decomposition method, and alpha is a preset weight;
and if the numerical value of the reference pitch angle is larger than a preset pitch angle reference value, executing the step of establishing a closed loop feedback loop by taking a preset strapdown inertial navigation system error angle and angular speed as input and a follow-up angle as output according to the incidence relation between the follow-up angle and gain, damping ratio and natural angular frequency of a strapdown inertial navigation system loop, wherein the pitch angle reference value is 60-70 degrees.
With reference to the first aspect, an embodiment of the present invention provides a fifth possible implementation manner of the first aspect, where the method further includes:
searching by adopting a genetic algorithm according to a preset initial value of the course angle and an initial value of the rolling angle;
when the difference between the searched course angle change value and the course angle actual value is smaller than the preset course difference, and the difference between the rolling angle change value and the rolling angle actual value is smaller than the preset rolling difference, determining that the difference between the course angle change value and the course angle actual value at the moment is the error compensation angle of the course angle, and determining that the difference between the rolling angle change value and the rolling angle actual value is the error compensation angle of the rolling angle.
With reference to the first aspect, an embodiment of the present invention provides a sixth possible implementation manner of the first aspect, where the obtaining an attitude angle according to the first optimized strapdown matrix includes:
acquiring a mathematical platform misalignment angle of the strapdown inertial navigation system in a Kalman filtering mode;
correcting the first optimized strapdown matrix according to the mathematical platform misalignment angle and the optimized follow-up angle to determine a second optimized strapdown matrix;
deriving a pose angle from the second optimized strapdown matrix.
With reference to the first aspect, an embodiment of the present invention provides a seventh possible implementation manner of the first aspect, where the obtaining a mathematical platform misalignment angle of the strapdown inertial navigation system in a kalman filter manner includes:
discretizing a pre-acquired state equation and an observation equation of the integrated navigation system, and performing recursive estimation through Kalman filtering to determine an optimal state estimation equation;
and determining the mathematical plateau misalignment angle according to the state-optimal estimation equation.
With reference to the first aspect, an embodiment of the present invention provides an eighth possible implementation manner of the first aspect, where correcting the first optimized strapdown matrix according to the mathematical misalignment angle of the platform and the optimized following angle to determine a second optimized strapdown matrix includes:
correcting the first optimized strapdown matrix according to the misalignment angle of the mathematical platform to determine an incompletely optimized strapdown matrix;
and correcting the incompletely optimized strapdown matrix again according to the optimized follow-up angle so as to determine the second optimized strapdown matrix.
With reference to the first aspect, an embodiment of the present invention provides a ninth possible implementation manner of the first aspect, where the method further includes calculating a reckoning velocity and a reckoning position of the strapdown inertial navigation system according to the following formulas, <math>
<mrow>
<mfenced open='{' close=''>
<mtable>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>x</mi>
<mo>·</mo>
</mover>
<mi>e</mi>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<msubsup>
<mi>v</mi>
<mi>e</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>v</mi>
<mo>·</mo>
</mover>
<mi>e</mi>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<msup>
<msub>
<mi>C</mi>
<mi>b</mi>
</msub>
<mi>n</mi>
</msup>
<msup>
<mi>f</mi>
<mi>b</mi>
</msup>
<mo>-</mo>
<mo>[</mo>
<msubsup>
<mrow>
<mn>2</mn>
<mi>ω</mi>
</mrow>
<mi>ie</mi>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>ω</mi>
<mi>en</mi>
<mi>n</mi>
</msubsup>
<mo>]</mo>
<mo>×</mo>
<msubsup>
<mi>v</mi>
<mi>e</mi>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msup>
<mi>g</mi>
<mi>n</mi>
</msup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
</math> whereinIs a vector of the measured and calculated position,is to measure the velocity vector, fbIs a specific force signal which is a specific force signal,is a strapdown matrixInverse matrix of gn=[0,0,g]TIs the vector of the gravity of the earth,is the projection of the angular velocity omega of the earth rotation in a navigation coordinate system,for navigating a coordinate systemAngular velocity relative to a terrestrial coordinate system;
respectively calculating a speed error and a position error according to the measured speed vector, the measured position vector and a detected speed vector and a detected position vector which are acquired by a satellite receiver in advance;
and respectively obtaining a system state equation and an observation equation by taking the speed error and the position error as observation variables and taking a pre-selected mathematical platform misalignment angle, a gyroscope zero offset, a gyroscope random drift rate and an accelerometer zero offset as a combined system error state vector.
The attitude angle calculation method in the integrated navigation provided by the embodiment of the invention adopts a follow-up angle correction mode, compared with the prior art that when the pitch angle is too large, the attitude angle of the integrated navigation system is difficult to accurately obtain, and further the navigation error is too large, the method comprises the steps of firstly taking a pre-acquired strapdown inertial navigation system error angle and angular velocity as input, taking a follow-up angle as output, establishing a closed loop feedback loop according to the incidence relation between the follow-up angle and the gain, the damping ratio and the natural angular frequency of the strapdown inertial navigation system loop, further acquiring a follow-up angle with convergence characteristics as an optimized follow-up angle, further correcting the original strapdown matrix according to the optimized follow-up angle, the first optimized strapdown matrix is determined, and finally the attitude angle is obtained according to the first optimized strapdown matrix, so that the calculation of the more accurate attitude angle is completed, and the defects in the prior art are overcome.
In order to make the aforementioned and other objects, features and advantages of the present invention comprehensible, preferred embodiments accompanied with figures are described in detail below.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings needed to be used in the embodiments will be briefly described below, it should be understood that the following drawings only illustrate some embodiments of the present invention, and therefore should not be considered as limiting the scope, and for those skilled in the art, other related drawings can be obtained according to the drawings without inventive efforts.
FIG. 1 is a basic flowchart of a method for calculating attitude angles in integrated navigation according to an embodiment of the present invention;
FIG. 2 illustrates a method for computing attitude angles in integrated navigation provided by embodiments of the present invention
A servo feedback loop schematic diagram of the method;
FIG. 3 illustrates a method for computing attitude angles in integrated navigation provided by embodiments of the present invention
Method for determining error compensation angle of course angle and error compensation of rolling angle by using genetic algorithm
A flow diagram of angle compensation;
FIG. 4 illustrates a method for computing attitude angles in integrated navigation provided by embodiments of the present invention
Further acquiring more accurate attitude on the basis of the strapdown matrix after the first optimization
A state angle refinement flow chart;
FIG. 5 illustrates a method for computing attitude angles in integrated navigation provided by embodiments of the present invention
The overall logic feedback control schematic diagram of the method.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. The components of embodiments of the present invention generally described and illustrated in the figures herein may be arranged and designed in a wide variety of different configurations. Thus, the following detailed description of the embodiments of the present invention, presented in the figures, is not intended to limit the scope of the invention, as claimed, but is merely representative of selected embodiments of the invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments of the present invention without making any creative effort, shall fall within the protection scope of the present invention.
The embodiment of the invention provides a method for calculating an attitude angle in integrated navigation, which comprises the following steps as shown in figure 1:
s101, taking a pre-acquired strapdown inertial navigation system error angle and angular velocity as input, taking a follow-up angle as output, and establishing a closed-loop feedback loop according to the incidence relation between the follow-up angle and gain, damping ratio and natural angular frequency of a strapdown inertial navigation system loop, wherein the error angle is a difference value between a pitch angle and a preset critical angle;
s102, calculating an optimized follow-up angle in a preset state according to a transfer function formula of a closed loop feedback loop;
s103, correcting the pre-acquired original strapdown matrix by using the optimized follow-up angle to determine a first optimized strapdown matrix;
and S104, acquiring an attitude angle according to the first optimized strapdown matrix.
In step S101, in order to obtain a good convergence characteristic of the servo angle, it is determined by designing a closed-loop feedback loop with the servo angle as an output inside the system. Specifically, the dynamic response characteristic of the servo feedback loop (closed-loop feedback loop) is designed to be equal to the gain K, the damping ratio ζ and the natural angular frequency ωnIt is related. The input and output satisfy the following linear differential equation:
Step S102, the system model shown in FIG. 2 has only three adjustable parameters K, ζ, ωnThe servo feedback loop has simple structure and convenient debugging, and can easily achieve the expected dynamic response characteristic. The third-order transfer function matrix of the input and the output satisfies the following expression:
Wherein calculating the optimized follow-up angle in the predetermined state according to the transfer function formula of the closed-loop feedback loop comprises:
discretizing the transfer function expression of the closed loop feedback loop according to a pre-acquired sampling period to determine a recursion formula for optimizing the follow-up angle; and calculating the optimized follow-up angle according to a recursion formula of the optimized follow-up angle. That is, when the combined navigation computer is used for follow-up feedback loop calculation, if the data sampling period is TsUsing bilinear transformation equationsDiscretization is carried out, so that a recursion calculation formula of the follow-up angle is obtained:
Δθ(k)=Δθ1(k)+Δθ2(k);
Δθ1(k)=a1Δθ1(k-1)+a2Δθ1(k-2)+b0θ(k)+b1θ(k-1)+b2θ(k-2);
Δθ2(k)=c1Δθ2(k-1)+c2Δθ2(k-2)+c3Δθ2(k-3)+d0ωx(k)+d1ωx(k-1)+d2ωx(k-2)+d3ωx(k-3);
in the formula <math>
<mrow>
<msub>
<mi>a</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mn>8</mn>
<mo>+</mo>
<msubsup>
<mrow>
<mn>2</mn>
<mi>Kω</mi>
</mrow>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>a</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mo>-</mo>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>-</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>b</mi>
<mn>0</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>θ</mi>
</mrow>
<mi>c</mi>
</msub>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
</mrow>
</math> <math>
<mrow>
<msub>
<mi>b</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mrow>
<mn>2</mn>
<mi>ω</mi>
</mrow>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>b</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mn>12</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mrow>
<mn>6</mn>
<mi>Kω</mi>
</mrow>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
</mrow>
</math> <math>
<mrow>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mo>-</mo>
<mn>12</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>-</mo>
<msubsup>
<mrow>
<mn>3</mn>
<mi>Kω</mi>
</mrow>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>c</mi>
<mn>3</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mn>4</mn>
<mo>-</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>d</mi>
<mn>0</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>3</mn>
</msubsup>
</mrow>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
<mo>,</mo>
</mrow>
</math> <math>
<mrow>
<msub>
<mi>d</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mrow>
<mn>3</mn>
<mi>ω</mi>
</mrow>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>3</mn>
</msubsup>
</mrow>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>d</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mrow>
<mn>3</mn>
<mi>ω</mi>
</mrow>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>3</mn>
</msubsup>
</mrow>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>d</mi>
<mn>3</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>3</mn>
</msubsup>
</mrow>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<mi>K</mi>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
<mo>,</mo>
</mrow>
</math> Wherein K is the gain of the loop of the strapdown inertial navigation system, ζ is the damping ratio of the loop of the strapdown inertial navigation system, ωnIs the natural angular frequency of the loop of the strapdown inertial navigation system. Therefore, the follow-up angle in the convergence state can be rapidly obtained through a recursion formula of the follow-up angle, namely the follow-up angle is optimized.
Step S103, the pre-obtained strapdown matrix needs to be corrected according to the optimized follow-up angle, so as to determine a more accurate strapdown matrix. Specifically, before executing step S103, step S105 may be further included, where the strapdown matrix of the current strapdown inertial navigation system is modified according to the error compensation angle of the heading angle and the error compensation angle of the roll angle, which are obtained in advance, so as to determine the original strapdown matrix.
The strapdown matrix of the strapdown inertial navigation system is a trigonometric function matrix about a heading angle, a pitch angle and a rolling angle. Let the current strapdown matrix beThe error compensation angles of the course angle psi and the roll angle gamma of the moving carrier are respectively delta psi0、Δγ0(the above parametersΔψ0And Δ γ0Is obtainable by existing means, can be used directly), by following a small angleBy the error compensation angle delta phi of the course angle0Error compensation angle delta gamma of rolling angle0Strapdown matrix for current strapdown inertial navigation systemAnd correcting to obtain the original strapdown matrix. The specific correction formula is as follows: <math>
<mrow>
<msubsup>
<mi>C</mi>
<mi>n</mi>
<mrow>
<mi>b</mi>
<mn>1</mn>
</mrow>
</msubsup>
<mo>=</mo>
<mfenced open='[' close=']'>
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<msub>
<mrow>
<mo>-</mo>
<mi>Δψ</mi>
</mrow>
<mn>0</mn>
</msub>
</mtd>
<mtd>
<mo>-</mo>
<msub>
<mi>Δγ</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Δψ</mi>
<mn>0</mn>
</msub>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mo>-</mo>
<msub>
<mi>Δψ</mi>
<mn>0</mn>
</msub>
<msub>
<mi>Δγ</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Δγ</mi>
<mn>0</mn>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<msubsup>
<mi>C</mi>
<mi>n</mi>
<mrow>
<mi>b</mi>
<mn>0</mn>
</mrow>
</msubsup>
<mo>,</mo>
</mrow>
</math> in the formula,. DELTA.psi0Angle of error compensation, Δ γ, for course angle0The angle is compensated for errors in the roll angle,is a strap-down matrix of the current strap-down inertial navigation system,is the original strapdown matrix.
Further, the original strapdown matrix is corrected again by using the coordinate transformation relation under a small angle and the optimized follow-up angle Δ θ obtained in step S102 to determine a first optimized strapdown matrix. The following formula for correction by using the following angle is as follows: <math>
<mrow>
<msubsup>
<mi>C</mi>
<mi>n</mi>
<mi>b</mi>
</msubsup>
<mo>=</mo>
<mfenced open='[' close=']'>
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mi>Δθ</mi>
<mo>)</mo>
</mrow>
</mtd>
<mtd>
<mo>-</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mi>Δθ</mi>
<mo>)</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mi>Δθ</mi>
<mo>)</mo>
</mrow>
</mtd>
<mtd>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mi>Δθ</mi>
<mo>)</mo>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<msubsup>
<mi>C</mi>
<mi>n</mi>
<mrow>
<mi>b</mi>
<mn>1</mn>
</mrow>
</msubsup>
<mo>,</mo>
</mrow>
</math> in the formula, delta theta is an optimized follow-up angle,is the original strapdown matrix and is used as a matrix,the strapdown matrix is optimized for the first.
First optimized strapdown matrixAs a new updated strapdown matrix of the strapdown inertial navigation system, the influence of a large pitch angle is overcome, so that the calculation of the speed, the position and the attitude angle of the strapdown inertial navigation system is more accurate, that is, step S104 may be executed to obtain the attitude angle according to the first optimized strapdown matrix, thereby completing the obtaining of the accurate attitude angle, wherein the obtained attitude angle refers to one or more of a course angle, a pitch angle and a roll angle.
It should be noted that the attitude angle calculation method in the integrated navigation provided by the present invention is preferably applied to the case where the pitch angle of the motion carrier is large, and certainly, is also applicable to the case where the pitch angle is low, and can still perform the function of effectively correcting the attitude angle in the case where the pitch angle of the motion carrier is small.
In order to reduce the calculation amount of the system, before executing step S101, whether to execute step S101-step S103 may be determined by judging the current pitch angle of the system in advance, so that when the pitch angle reaches a certain requirement, step S101 and the subsequent steps may be selected and executed according to specific situations.
That is, the method of the present invention further comprises the step of calculating the reference according to the following formulaPitch angle, theta ═ alpha thetas+(1-α)θaWherein, thetasFor a first reference pitch angle, theta, calculated by quaternionaA second reference pitch angle is calculated according to an acceleration decomposition method, and alpha is a preset weight;
and if the numerical value of the reference pitch angle is larger than the preset pitch angle reference value, the execution step takes a preset strapdown inertial navigation system error angle and angular speed as input, a follow-up angle as output, and a closed-loop feedback loop is established according to the incidence relation between the follow-up angle and the gain, damping ratio and natural angular frequency of the strapdown inertial navigation system loop, wherein the pitch angle reference value is 60-70 degrees.
Wherein, the current acceleration a (after correction) of the motion carrier can be [ nabis ] through the accelerometerx,▽y,▽z]TAnd correcting the actual measurement value to obtain more accurate acceleration of the moving carrier. In a three-dimensional coordinate system, vectors such as acceleration, angular velocity and the like can be decomposed into three components along the coordinate axes of the three-dimensional coordinate system, namely a obtained by actual measurementm=[amx,amy,amz]TAnd then corrected acceleration <math>
<mrow>
<mfenced open='[' close=']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>a</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>a</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>a</mi>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open='[' close=']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>a</mi>
<mi>mx</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>a</mi>
<mi>my</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>a</mi>
<mi>mz</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mfenced open='[' close=']'>
<mtable>
<mtr>
<mtd>
<msub>
<mo>▿</mo>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mo>▿</mo>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mo>▿</mo>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
</math> I.e. a ═ ax,ay,az]TThe corrected acceleration is subjected to vector operation to obtain a numerical value (absolute acceleration) of the corrected acceleration as
After obtaining the corrected acceleration a ═ ax,ay,az]TThen, a second reference pitch angle is calculated according to an acceleration decomposition methodWherein g is the acceleration of gravity, ayIs the component of the acceleration in the vertical direction.
The value of the acceleration after correction (absolute acceleration) is obtained asThen, a common quaternion method can be calculated according to the attitude angle of the strapdown inertial navigation system to calculate a first reference pitch angle thetas=arcsin[2(q2q3-q0q1)]Wherein q is0、q1、q2And q is3Respectively, elements in a quaternion.
In order to obtain more accurate reference pitch angle, the reference pitch angle can be obtained by respectively aligning the first reference pitch angle thetasAnd a second reference pitch angle thetaaAnd giving different weights to adjust the value of the final reference pitch angle theta. Specifically, the formula θ ═ α θ is useds+(1-α)θaTo calculate a reference pitch angle theta, where thetasIs a first reference pitch angle, thetaaIs a second reference pitch angle. When the current absolute acceleration a of the moving carrier is small, the weighting factor is larger, so that | a-g |, > is less<When 0.5, α may be 0.8, otherwise α may be 0.2. Of course, by referencing the first reference pitch angle θsAnd a second reference pitch angle thetaaThe weighting method may be performed to determine the reference pitch angle θ, which may be a first reference pitch angle θsAnd a second reference pitch angle thetaaThe way that the weight does not produce correlation, e.g. theta ═ alpha1θs+α2θa。
When the pitch angle of the motion carrier is larger than 60 degrees, the performance of the integrated navigation is seriously deteriorated, and a threshold value theta of the pitch angle can be setcTaking the value in the range of (60 degrees and 70 degrees) as a judgment condition. The corrected pitch angle theta is larger than a set critical value thetacI.e. theta>θcThen, the servo feedback loop is started to solve, that is, step S101 may be executed.
And S105, correcting the strapdown matrix of the current strapdown inertial navigation system according to the error compensation angle of the pre-acquired course angle and the error compensation angle of the rolling angle to determine the original strapdown matrix. The method can comprise the following steps: searching by adopting a genetic algorithm according to a preset initial value of the course angle and an initial value of the rolling angle;
when the difference between the searched course angle change value and the course angle actual value is smaller than the preset course difference, and the difference between the rolling angle change value and the rolling angle actual value is smaller than the preset rolling difference, determining that the difference between the course angle change value and the course angle actual value at the moment is the error compensation angle of the course angle, and determining that the difference between the rolling angle change value and the rolling angle actual value is the error compensation angle of the rolling angle.
Namely, the error compensation angle delta phi of the course angle and the roll angle of the moving carrier in the process of correcting the original strapdown matrix by utilizing the follow-up angle0、Δγ0Obtained by off-line computational genetic algorithm search. The stopping conditions of the genetic algorithm search are as follows: the differences between the course angle psi and the roll angle gamma of the motion carrier calculated off-line and the corresponding true values (the known roll angle actual value and course angle actual value) are all within a certain small angle range, namely delta psi0=|ψ-ψTrue|<Beta, and delta gamma0=|γ-γTrue|<If the value of beta and beta is generally less than 5 degrees, the difference value between the change value of the course angle and the actual value of the course angle at the moment is determined as the error compensation angle delta psi of the course angle0And determining the difference between the roll angle variation value and the roll angle actual value as the error compensation angle delta gamma of the roll angle0. It should be noted that, when searching by using the genetic algorithm, an initial value needs to be determined first, and when searching by the system, according to the continuous variation value of the initial value (which means the continuous variation of the values of the roll angle and the course angle, that is, a set of roll angle variation value and course angle variation value is generated for each variation), that is, after each variation, it is determined whether the difference between the course angle variation value and the course angle actual value is smaller than the preset course difference, and whether the difference between the roll angle variation value and the roll angle actual value is smaller than the preset roll difference, and when both determinations are true, it is determined that the difference between the course angle variation value and the course angle actual value at this time is the error compensation angle Δ ψ of the course angle0And determining the difference between the roll angle variation value and the roll angle actual value as the error compensation angle delta gamma of the roll angle0。
Specifically, the angular velocity and acceleration information acquired in the whole process and known heading angles and rolling angle truth values of the moving carrier are utilized to perform off-line calculation genetic algorithm search on the error compensation angle.
Coding criteria of error compensation angles of course angle and rolling angle of the moving carrier: all adopt 8-bit binary coding, coding range [0x00H, 0xFFH ]. 0x00H for-5 degrees, 0x80H for 0 degrees, and 0xFFH for 5 degrees.
Convergence criterion of error compensation angle search of course angle and rolling angle of the moving carrier: phi-phiTrue|<β、γ-γTrue|<β。
When the error compensation angle is searched by a genetic algorithm, three operators of selection, intersection and variation are adopted for the binary code. The search process is shown in figure 3.
Search for Δ ψ0Is NΔψ0Then, thenAn error compensation angle of the heading angle is calculated.
Search for Δ γ0Is NΔγ0Then, thenThe error compensation angle for the roll angle is also calculated.
Specifically, as shown in fig. 4, the step S104 of obtaining the attitude angle according to the first optimized strapdown matrix includes the following steps:
s1041, acquiring a mathematical platform misalignment angle of the strapdown inertial navigation system in a Kalman filtering mode;
s1042, correcting the first optimized strapdown matrix according to the misalignment angle and the optimized follow-up angle of the mathematical platform to determine a second optimized strapdown matrix;
and S1043, deriving a posture angle from the second optimized strapdown matrix.
In step S1041, acquiring a mathematical platform misalignment angle of the strapdown inertial navigation system in a kalman filtering manner, which may specifically be divided into two steps, that is, step S1041, and acquiring the mathematical platform misalignment angle of the strapdown inertial navigation system in the kalman filtering manner includes:
discretizing a pre-acquired state equation and an observation equation of the integrated navigation system, and performing recursive estimation through Kalman filtering to determine an optimal state estimation equation;
and determining the mathematical platform misalignment angle according to the state optimal estimation equation.
The detailed process is to first obtain the mathematical plateau misalignment angle (phi)E、φN、φU) Velocity error (V)E、VN、VU) Position error (L, lambda, h), gyroscope zero offset (bx、by、bz) Gyroscope random drift rate (rx、ry、rz) Zero bias ([ v ]) of accelerometerx、▽y、▽z) As an error state vector of the combined navigation system, the difference between the position and the speed is used as an observation variable, and a system state equation and an observation equation can be obtained: <math>
<mrow>
<mi>Z</mi>
<mo>=</mo>
<mfenced open='[' close=']'>
<mtable>
<mtr>
<mtd>
<mi>δR</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>δV</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mi>HX</mi>
<mo>+</mo>
<msub>
<mi>V</mi>
<mi>k</mi>
</msub>
<mo>,</mo>
</mrow>
</math> wherein, f (t) is a state transition matrix of the strapdown inertial navigation system, g (t) is a noise input matrix of the strapdown inertial navigation system, w (t) is system noise, and R ═ L, λ, h]T(position error), V ═ VE,VNVU]T(speed error), VkIs the measurement noise.
Then, discretizing the state equation and the observation equation respectively to obtain: x (k +1) ═ Φ (k +1, k) X (k) + (k +1, k) w (k); z (k) ═ h (k) x (k) + v (k). Thereby obtaining a state stepPrediction equationAnd the optimal state estimation equation: wherein, the filter gain equation is:
K(k+1)=P(k+1/k)HT(k+1)[H(k+1)P(k+1/k)HT(k+1)+Rk+1]-1;
the one-step prediction mean square error equation is as follows:
P(k+1/k)=φ(k+1,k)P(k/k)φT(k+1,k)+(k+1,k)Qk T(k+1,k);
the estimated mean square error equation is:
p (K +1/K +1) ═ I-K (K +1) H (K +1) ] P (K + 1/K). And performing Kalman filtering recursion estimation by using the discretized system model to obtain an optimal estimation value of the error state vector, namely obtaining a mathematical platform misalignment angle (the optimal estimation value of the mathematical platform misalignment angle). In step S1042, the first optimized strapdown matrix is corrected according to the mathematical platform misalignment angle and the optimized following angle to determine a second optimized strapdown matrix, which may be divided into the following two steps:
correcting the first optimized strapdown matrix according to the misalignment angle of the mathematical platform to determine an incompletely optimized strapdown matrix;
and correcting the incompletely optimized strapdown matrix again according to the optimized follow-up angle so as to determine a second optimized strapdown matrix.
When the first optimized strapdown matrix is corrected by using the mathematical platform misalignment angle and the optimized follow-up angle, the follow-up angle can be used for correction firstly, and then the mathematical platform misalignment angle is used for correction, or the mathematical platform misalignment angle can be used for correction firstly and then the follow-up angle is used for correction; but the preferred scheme is to use the mathematical platform misalignment angle to correct firstly, then use the following angle to correct, the concrete formula of using the following angle to correct is as follows: <math>
<mrow>
<msubsup>
<mi>C</mi>
<mi>n</mi>
<mi>bt</mi>
</msubsup>
<mo>=</mo>
<mfenced open='[' close=']'>
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mi>Δθ</mi>
<mo>)</mo>
</mrow>
</mtd>
<mtd>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mi>Δθ</mi>
<mo>)</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mo>-</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mi>Δθ</mi>
<mo>)</mo>
</mrow>
</mtd>
<mtd>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mi>Δθ</mi>
<mo>)</mo>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<msubsup>
<mi>C</mi>
<mi>n</mi>
<mi>b</mi>
</msubsup>
<mo>,</mo>
</mrow>
</math> whereinFor the first optimized strapdown matrix that has been previously found,for the second optimized strapdown matrix found, Δ θ is the follow-up angle, which has been found in step S102.
It should be noted that, the kalman filter recurses and calculates the front gyroscope zero offset (bx、by、bz) Gyroscope random drift rate (rx、ry、rz) Zero bias ([ v ]) of accelerometerx、▽y、▽z) Has been obtained in advance, and also needs to calculate the speed error (V)E、VN、VU) Position error (L, lambda, h). The velocity error is obtained by subtracting the velocity obtained by the strapdown inertial navigation system according to the basic equation and the velocity obtained by the satellite, the position error is obtained by subtracting the position obtained by the strapdown inertial navigation system according to the basic equation and the position obtained by the satellite, and the velocity and position obtained by the calculation of the strapdown inertial navigation system are calculated by the following algorithm: <math>
<mrow>
<mfenced open='{' close=''>
<mtable>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>x</mi>
<mo>·</mo>
</mover>
<mi>e</mi>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<msubsup>
<mi>v</mi>
<mi>e</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>v</mi>
<mo>·</mo>
</mover>
<mi>e</mi>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<msup>
<msub>
<mi>C</mi>
<mi>b</mi>
</msub>
<mi>n</mi>
</msup>
<msup>
<mi>f</mi>
<mi>b</mi>
</msup>
<mo>-</mo>
<mo>[</mo>
<msubsup>
<mrow>
<mn>2</mn>
<mi>ω</mi>
</mrow>
<mi>ie</mi>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>ω</mi>
<mi>en</mi>
<mi>n</mi>
</msubsup>
<mo>]</mo>
<mo>×</mo>
<msubsup>
<mi>v</mi>
<mi>e</mi>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msup>
<mi>g</mi>
<mi>n</mi>
</msup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
</math> wherein,is a vector of the position of the object,is a velocity vector, fbIs a specific force signal which is a specific force signal,is a strapdown matrixInverse matrix of gn=[0,0,g]TIs the vector of the gravity of the earth,is the projection of the angular velocity omega of the earth rotation in a navigation coordinate system,is the angular velocity of the navigational coordinate system relative to the terrestrial coordinate system. The position error and the speed error can be obtained by using the observation results (speed and position) obtained by the satellite, and the misalignment angle (phi) of the mathematical platform can be further obtainedE、φN、φU) Zero-bias gyroscopebx、by、bz) Gyroscope random drift rate (rx、ry、rz) Zero bias ([ v ]) of accelerometerx、▽y、▽z) As a combined navigation system error state vector, velocity error (V)E、VN、VU) And the position error (L, lambda and h) are used as observation variables, and a system state equation and an observation equation can be obtained.
Referring to the attitude angle calculation method in integrated navigation provided by the invention as a whole, as shown in fig. 5, an inertial navigation measurement unit provides original angular velocity and acceleration measurement information, and a satellite receiver provides reference velocity and position information (used when calculating a velocity error and a position error), and an integrated navigation algorithm mainly comprises three major parts, namely a strapdown inertial navigation system solution, a kalman filter optimal estimation and a follow-up feedback loop. These three parts of the algorithm interact, interacting. Therefore, the follow-up angle in the convergence state is obtained from the follow-up feedback loop, the strapdown matrix is further corrected according to the follow-up angle, the mathematical platform misalignment angle is used for correcting the strapdown matrix, and therefore the accurate attitude angle is extracted from the optimized strapdown matrix.
The above description is only for the specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present invention, and all the changes or substitutions should be covered within the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.
Claims (10)
1. The attitude angle calculation method in the integrated navigation is characterized by comprising the following steps:
taking a pre-acquired strapdown inertial navigation system error angle and angular velocity as input, taking a follow-up angle as output, and establishing a closed loop feedback loop according to the incidence relation between the follow-up angle and gain, damping ratio and natural angular frequency of a strapdown inertial navigation system loop, wherein the error angle is the difference value between a pitch angle and a preset critical angle;
calculating an optimized follow-up angle under a preset state according to the transfer function formula of the closed loop feedback loop;
correcting the pre-acquired original strapdown matrix by using the optimized follow-up angle to determine a first optimized strapdown matrix;
and acquiring an attitude angle according to the first optimized strapdown matrix.
2. The attitude angle calculation method in integrated navigation according to claim 1, characterized by further comprising:
and correcting the strapdown matrix of the current strapdown inertial navigation system according to the error compensation angle of the course angle and the error compensation angle of the rolling angle which are acquired in advance to determine the original strapdown matrix.
3. The attitude angle calculation method in integrated navigation according to claim 1, wherein the calculating an optimized follow-up angle in a predetermined state according to the transfer function expression of the closed-loop feedback loop includes:
discretizing the transfer function formula of the closed loop feedback loop according to a pre-acquired sampling period to determine a recursion formula for optimizing a follow-up angle;
and calculating the optimized follow-up angle according to the recursion formula of the optimized follow-up angle.
4. The attitude angle calculation method in integrated navigation according to claim 3, characterized in that the recurrence formula is Δ θ (k) ═ Δ θ1(k)+Δθ2(k),Δθ1(k)=a1Δθ1(k-1)+a2Δθ1(k-2)+b0θ(k)+b1θ(k-1)+b2θ(k-2),Δθ2(k)=c1Δθ2(k-1)+c2Δθ2(k-2)+c3Δθ2(k-3)+d0ωx(k)+d1ωx(k-1)+d2ωx(k-2)+d3ωx(k-3) wherein, <math>
<mrow>
<msub>
<mi>a</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mn>8</mn>
<mo>+</mo>
<mn>2</mn>
<mi>K</mi>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<mi>ζ</mi>
<msub>
<mi>ω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>a</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mo>-</mo>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>-</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>b</mi>
<mn>0</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<mn>4</mn>
<msub>
<mi>θ</mi>
<mi>c</mi>
</msub>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<mi>ζ</mi>
<msub>
<mi>ω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
</mrow>
</math> <math>
<mrow>
<msub>
<mi>b</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mn>2</mn>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<msub>
<mrow>
<mn>4</mn>
<mi>ζω</mi>
</mrow>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>b</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<mi>ζ</mi>
<msub>
<mi>ω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>c</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mn>12</mn>
<mo>+</mo>
<mn>4</mn>
<mi>ζ</mi>
<msub>
<mi>ω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<mn>6</mn>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
</mrow>
</math> <math>
<mrow>
<msub>
<mi>c</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mo>-</mo>
<mn>12</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>-</mo>
<msubsup>
<mrow>
<mn>3</mn>
<mi>Kω</mi>
</mrow>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>c</mi>
<mn>3</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mn>4</mn>
<mo>-</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>d</mi>
<mn>0</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>3</mn>
</msubsup>
</mrow>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
<mo>,</mo>
</mrow>
</math> <math>
<mrow>
<msub>
<mi>d</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mrow>
<mn>3</mn>
<mi>ω</mi>
</mrow>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>3</mn>
</msubsup>
</mrow>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>d</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mrow>
<mn>3</mn>
<mi>ω</mi>
</mrow>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>3</mn>
</msubsup>
</mrow>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>d</mi>
<mn>3</mn>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mi>ω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<msubsup>
<mi>T</mi>
<mi>s</mi>
<mn>3</mn>
</msubsup>
</mrow>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>+</mo>
<mn>4</mn>
<msub>
<mi>ζω</mi>
<mi>n</mi>
</msub>
<mo>+</mo>
<msubsup>
<mi>Kω</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
<mo>,</mo>
</mrow>
</math> k is the gain of the loop of the strapdown inertial navigation system, zeta is the damping ratio of the loop of the strapdown inertial navigation system, omeganIs the natural angular frequency of the loop of the strapdown inertial navigation system.
5. The attitude angle calculation method in integrated navigation according to claim 1, characterized by further comprising:
calculating a reference pitch angle according to the following formula, theta ═ alpha thetas+(1-α)θaWherein, thetasFor a first reference pitch angle, theta, calculated by quaternionaA second reference pitch angle is calculated according to an acceleration decomposition method, and alpha is a preset weight;
and if the numerical value of the reference pitch angle is larger than a preset pitch angle reference value, executing the step of establishing a closed loop feedback loop by taking a preset strapdown inertial navigation system error angle and angular speed as input and a follow-up angle as output according to the incidence relation between the follow-up angle and gain, damping ratio and natural angular frequency of a strapdown inertial navigation system loop, wherein the pitch angle reference value is 60-70 degrees.
6. The attitude angle calculation method in integrated navigation according to claim 2, characterized by further comprising:
searching by adopting a genetic algorithm according to a preset initial value of the course angle and an initial value of the rolling angle;
when the difference between the searched course angle change value and the course angle actual value is smaller than the preset course difference, and the difference between the rolling angle change value and the rolling angle actual value is smaller than the preset rolling difference, determining that the difference between the course angle change value and the course angle actual value at the moment is the error compensation angle of the course angle, and determining that the difference between the rolling angle change value and the rolling angle actual value is the error compensation angle of the rolling angle.
7. The attitude angle calculation method in integrated navigation according to any one of claims 1 to 6, wherein the obtaining an attitude angle from the first optimized strapdown matrix includes:
acquiring a mathematical platform misalignment angle of the strapdown inertial navigation system in a Kalman filtering mode;
correcting the first optimized strapdown matrix according to the mathematical platform misalignment angle and the optimized follow-up angle to determine a second optimized strapdown matrix;
deriving a pose angle from the second optimized strapdown matrix.
8. The method for calculating the attitude angle in the integrated navigation according to claim 7, wherein the obtaining the mathematical platform misalignment angle of the strapdown inertial navigation system by using the kalman filter comprises:
discretizing a pre-acquired state equation and an observation equation of the integrated navigation system, and performing recursive estimation through Kalman filtering to determine an optimal state estimation equation;
and determining the mathematical plateau misalignment angle according to the state-optimal estimation equation.
9. The attitude angle calculation method in integrated navigation according to claim 8, wherein correcting the first optimized strapdown matrix according to the mathematical platform misalignment angle and an optimized follow-up angle to determine a second optimized strapdown matrix comprises:
correcting the first optimized strapdown matrix according to the misalignment angle of the mathematical platform to determine an incompletely optimized strapdown matrix;
and correcting the incompletely optimized strapdown matrix again according to the optimized follow-up angle so as to determine the second optimized strapdown matrix.
10. The attitude angle calculation method in integrated navigation according to claim 9, further comprising calculating a reckoning velocity and a reckoning position of the strapdown inertial navigation system according to the following formulas, <math>
<mrow>
<mfenced open='{' close=''>
<mtable>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>x</mi>
<mo>.</mo>
</mover>
<mi>e</mi>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<msubsup>
<mi>v</mi>
<mi>e</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>v</mi>
<mo>.</mo>
</mover>
<mi>e</mi>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<msup>
<msub>
<mi>C</mi>
<mi>b</mi>
</msub>
<mi>n</mi>
</msup>
<msup>
<mi>f</mi>
<mi>b</mi>
</msup>
<mo>-</mo>
<mo>[</mo>
<msubsup>
<mrow>
<mn>2</mn>
<mi>ω</mi>
</mrow>
<mi>ie</mi>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>ω</mi>
<mi>en</mi>
<mi>n</mi>
</msubsup>
<mo>]</mo>
<mo>×</mo>
<msubsup>
<mi>v</mi>
<mi>e</mi>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msup>
<mi>g</mi>
<mi>n</mi>
</msup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
</math> whereinIs a vector of the measured and calculated position,is to measure the velocity vector, fbIs a specific force signal which is a specific force signal,is a strapdown matrixInverse matrix of gn=[0,0,g]TIs the vector of the gravity of the earth,is the projection of the angular velocity omega of the earth rotation in a navigation coordinate system,the angular velocity of the navigation coordinate system relative to the terrestrial coordinate system;
respectively calculating a speed error and a position error according to the measured speed vector, the measured position vector and a detected speed vector and a detected position vector which are acquired by a satellite receiver in advance;
and respectively obtaining a system state equation and an observation equation by taking the speed error and the position error as observation variables and taking a pre-selected mathematical platform misalignment angle, a gyroscope zero offset, a gyroscope random drift rate and an accelerometer zero offset as a combined system error state vector.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510109100.2A CN104634348B (en) | 2015-03-12 | 2015-03-12 | Attitude angle computational methods in integrated navigation |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510109100.2A CN104634348B (en) | 2015-03-12 | 2015-03-12 | Attitude angle computational methods in integrated navigation |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104634348A true CN104634348A (en) | 2015-05-20 |
CN104634348B CN104634348B (en) | 2017-09-15 |
Family
ID=53213359
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510109100.2A Active CN104634348B (en) | 2015-03-12 | 2015-03-12 | Attitude angle computational methods in integrated navigation |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104634348B (en) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109212962A (en) * | 2017-07-03 | 2019-01-15 | 中车株洲电力机车研究所有限公司 | Acceleration calculation method, closed-loop control system and acceleration estimation device |
CN110873563A (en) * | 2018-08-30 | 2020-03-10 | 杭州海康机器人技术有限公司 | Cloud deck attitude estimation method and device |
CN113203415A (en) * | 2021-04-12 | 2021-08-03 | 北京航空航天大学 | Atomic gyro navigation system and navigation resolving method thereof |
CN117516465A (en) * | 2024-01-04 | 2024-02-06 | 北京神导科技股份有限公司 | Inertial navigation system attitude angle extraction method capable of avoiding jump |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101629826A (en) * | 2009-07-01 | 2010-01-20 | 哈尔滨工程大学 | Coarse alignment method for fiber optic gyro strapdown inertial navigation system based on single axis rotation |
RU2380656C1 (en) * | 2008-12-24 | 2010-01-27 | Олег Степанович Салычев | Integrated strapdown inertial and satellite navigation system on coarse sensors |
CN101893445A (en) * | 2010-07-09 | 2010-11-24 | 哈尔滨工程大学 | Rapid initial alignment method for low-accuracy strapdown inertial navigation system under swinging condition |
CN102486377A (en) * | 2009-11-17 | 2012-06-06 | 哈尔滨工程大学 | Method for acquiring initial course attitude of fiber optic gyro strapdown inertial navigation system |
CN102830414A (en) * | 2012-07-13 | 2012-12-19 | 北京理工大学 | Combined navigation method based on SINS/GPS (strapdown inertial navigation system/global position system) |
CN103017765A (en) * | 2012-12-06 | 2013-04-03 | 北京遥测技术研究所 | Yaw angle correction method and yaw angle correction device applied to a micro-mechanical integrated navigation system |
-
2015
- 2015-03-12 CN CN201510109100.2A patent/CN104634348B/en active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
RU2380656C1 (en) * | 2008-12-24 | 2010-01-27 | Олег Степанович Салычев | Integrated strapdown inertial and satellite navigation system on coarse sensors |
CN101629826A (en) * | 2009-07-01 | 2010-01-20 | 哈尔滨工程大学 | Coarse alignment method for fiber optic gyro strapdown inertial navigation system based on single axis rotation |
CN102486377A (en) * | 2009-11-17 | 2012-06-06 | 哈尔滨工程大学 | Method for acquiring initial course attitude of fiber optic gyro strapdown inertial navigation system |
CN101893445A (en) * | 2010-07-09 | 2010-11-24 | 哈尔滨工程大学 | Rapid initial alignment method for low-accuracy strapdown inertial navigation system under swinging condition |
CN102830414A (en) * | 2012-07-13 | 2012-12-19 | 北京理工大学 | Combined navigation method based on SINS/GPS (strapdown inertial navigation system/global position system) |
CN103017765A (en) * | 2012-12-06 | 2013-04-03 | 北京遥测技术研究所 | Yaw angle correction method and yaw angle correction device applied to a micro-mechanical integrated navigation system |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109212962A (en) * | 2017-07-03 | 2019-01-15 | 中车株洲电力机车研究所有限公司 | Acceleration calculation method, closed-loop control system and acceleration estimation device |
CN110873563A (en) * | 2018-08-30 | 2020-03-10 | 杭州海康机器人技术有限公司 | Cloud deck attitude estimation method and device |
CN113203415A (en) * | 2021-04-12 | 2021-08-03 | 北京航空航天大学 | Atomic gyro navigation system and navigation resolving method thereof |
CN117516465A (en) * | 2024-01-04 | 2024-02-06 | 北京神导科技股份有限公司 | Inertial navigation system attitude angle extraction method capable of avoiding jump |
CN117516465B (en) * | 2024-01-04 | 2024-03-19 | 北京神导科技股份有限公司 | Inertial navigation system attitude angle extraction method capable of avoiding jump |
Also Published As
Publication number | Publication date |
---|---|
CN104634348B (en) | 2017-09-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103743414B (en) | Initial Alignment Method between the traveling of vehicle-mounted SINS assisted by a kind of speedometer | |
CN101949703B (en) | Strapdown inertial/satellite combined navigation filtering method | |
CN101788296B (en) | SINS/CNS deep integrated navigation system and realization method thereof | |
CN112505737B (en) | GNSS/INS integrated navigation method | |
CN103994763B (en) | The SINS/CNS deep integrated navigation system of a kind of Marsokhod and its implementation | |
CN102519470B (en) | Multi-level embedded integrated navigation system and navigation method | |
CN102830414B (en) | Combined navigation method based on SINS/GPS (strapdown inertial navigation system/global position system) | |
CN104697520B (en) | Integrated gyro free strap down inertial navigation system and gps system Combinated navigation method | |
CN104635251A (en) | Novel INS (inertial navigation system)/ GPS (global position system) combined position and orientation method | |
Xue et al. | In-motion alignment algorithm for vehicle carried SINS based on odometer aiding | |
CN106052685A (en) | Two-stage separation fusion attitude and heading estimation method | |
CN111189442B (en) | CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method | |
CN106767797A (en) | A kind of inertia based on dual quaterion/GPS Combinated navigation methods | |
CN109612460B (en) | Plumb line deviation measuring method based on static correction | |
CN106840211A (en) | A kind of SINS Initial Alignment of Large Azimuth Misalignment On methods based on KF and STUPF combined filters | |
CN104634348B (en) | Attitude angle computational methods in integrated navigation | |
Fu et al. | In-motion alignment for a velocity-aided SINS with latitude uncertainty | |
CN112798021A (en) | Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter | |
CN107576327A (en) | Varistructure integrated navigation system design method based on Observable degree analysis of Beidou double | |
Xu et al. | An acoustic ranging measurement aided SINS/DVL integrated navigation algorithm based on multivehicle cooperative correction | |
Zuo et al. | A GNSS/IMU/vision ultra-tightly integrated navigation system for low altitude aircraft | |
Liu et al. | Improved path following for autonomous marine vehicles with low-cost heading/course sensors: Comparative experiments | |
Yan et al. | Cooperative navigation in unmanned surface vehicles with observability and trilateral positioning method | |
CN110375740B (en) | Vehicle navigation method, device, equipment and storage medium | |
Dahmane et al. | Controlling the degree of observability in GPS/INS integration land-vehicle navigation based on extended Kalman filter |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |