CN114111771A - Dynamic attitude measurement method of double-shaft stable platform - Google Patents
Dynamic attitude measurement method of double-shaft stable platform Download PDFInfo
- Publication number
- CN114111771A CN114111771A CN202111416781.9A CN202111416781A CN114111771A CN 114111771 A CN114111771 A CN 114111771A CN 202111416781 A CN202111416781 A CN 202111416781A CN 114111771 A CN114111771 A CN 114111771A
- Authority
- CN
- China
- Prior art keywords
- rotation
- attitude
- quaternion
- carrier
- angle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000691 measurement method Methods 0.000 title claims abstract description 10
- 238000000034 method Methods 0.000 claims abstract description 39
- 238000001914 filtration Methods 0.000 claims abstract description 21
- 230000001133 acceleration Effects 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 46
- 230000009466 transformation Effects 0.000 claims description 45
- 239000013598 vector Substances 0.000 claims description 33
- 238000004422 calculation algorithm Methods 0.000 claims description 30
- 238000004364 calculation method Methods 0.000 claims description 28
- 230000000694 effects Effects 0.000 claims description 27
- 238000005070 sampling Methods 0.000 claims description 23
- 238000005259 measurement Methods 0.000 claims description 20
- 230000008569 process Effects 0.000 claims description 13
- 238000012937 correction Methods 0.000 claims description 9
- 238000006243 chemical reaction Methods 0.000 claims description 8
- 230000005484 gravity Effects 0.000 claims description 6
- 238000005457 optimization Methods 0.000 claims description 6
- 230000026676 system process Effects 0.000 claims description 6
- 230000008859 change Effects 0.000 claims description 4
- 238000005096 rolling process Methods 0.000 claims description 4
- 239000000470 constituent Substances 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 3
- 230000001627 detrimental effect Effects 0.000 claims description 3
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 230000007246 mechanism Effects 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 7
- 230000004927 fusion Effects 0.000 description 3
- 238000009529 body temperature measurement Methods 0.000 description 2
- 230000000295 complement effect Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 239000013307 optical fiber Substances 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000006641 stabilisation Effects 0.000 description 2
- 238000011105 stabilization Methods 0.000 description 2
- 230000000087 stabilizing effect Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- HEZMWWAKWCSUCB-PHDIDXHHSA-N (3R,4R)-3,4-dihydroxycyclohexa-1,5-diene-1-carboxylic acid Chemical compound O[C@@H]1C=CC(C(O)=O)=C[C@H]1O HEZMWWAKWCSUCB-PHDIDXHHSA-N 0.000 description 1
- 239000000969 carrier Substances 0.000 description 1
- 230000015271 coagulation Effects 0.000 description 1
- 238000005345 coagulation Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 239000010453 quartz Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- VYPSYNLAJGMNEJ-UHFFFAOYSA-N silicon dioxide Inorganic materials O=[Si]=O VYPSYNLAJGMNEJ-UHFFFAOYSA-N 0.000 description 1
- 238000007711 solidification Methods 0.000 description 1
- 230000008023 solidification Effects 0.000 description 1
- 238000003860 storage Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/53—Determining attitude
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
A dynamic attitude measurement method of a biaxial stable platform adopts Kalman filtering technology to carry out initial alignment, combined navigation and combined calibration of a strapdown inertial navigation system, estimates out misalignment angles, speed errors and position errors, corrects quaternion, resolving speed and resolving positions of the strapdown inertial navigation system by using the initial alignment, the combined navigation and the combined calibration, ensures that a calculated navigation coordinate system and a real navigation coordinate system coincide as much as possible, and hopes to estimate out errors (gyroscope drift and acceleration bias) of inertial elements under the condition of time allowance. The method can ensure that the double-shaft stable platform can still keep stable precision of a longitudinal rocking angle and a transverse rocking angle less than or equal to 10' (RMS) under the large load condition of loading an 80kg gravimeter and the large dynamic condition of a four-stage sea state.
Description
Technical Field
The invention relates to the field of measurement of gravimeters, in particular to a dynamic attitude measurement method of a double-shaft stable platform of a gravimeter.
Background
The two-axis stable platform keeps the frame platform stable at the local geographical horizontal attitude to isolate the influence of horizontal attitude disturbance on the gravimeter, and the attitude angle measurement of the previous two-axis stable platform is obtained by the complementary filtering fusion of data of three accelerometers and a two-axis horizontal gyroscope. And the two horizontal axis motors of the rolling and the pitching are used for carrying out geographic horizontal stabilization through the angular speed of the gyroscope and the inclination angle of the accelerometer.
In the reference literature, "control system for stabilizing platform of marine gravimeter" (measurement and geophysical research institute of chinese academy of sciences, wupeng flight, wanlong, zhou, wang courage, control system for stabilizing platform of marine gravimeter, application (patent) No. 201620193744.4, 2016.03.14), a method of directly performing control through data fusion of accelerometer and gyroscope is adopted, and in addition, an angle sensor is additionally added, so that the requirements on structure processing precision are high, the difficulty is high, the noise of the measurement sensor is large, and the requirements on high-precision attitude measurement are difficult to achieve.
Disclosure of Invention
The invention aims to provide a dynamic attitude measurement method of a biaxial stable platform, which is obtained by complementary filtering fusion of data of an accelerometer and a gyroscope, so that not only is filtering delay caused in large dynamic state, but also external linear acceleration is easy to interfere with. The stable platform can only achieve angular grading precision and is not suitable for high-precision occasions. The invention provides an attitude measurement method based on inertia/satellite navigation (INS/GPS) combination, which avoids measurement errors caused by filter delay and accelerometer interference through algorithm design, and can still keep a stable platform of a gravimeter to have 10' stable accuracy under the large dynamic conditions of 80kg large load and four-level sea condition.
The technical scheme adopted for achieving the purpose is that a dynamic attitude measurement method of a double-shaft stable platform adopts Kalman filtering technology to carry out initial alignment, integrated navigation and integrated calibration of a strapdown inertial navigation system, estimates a misalignment angle, a speed error and a position error, and corrects quaternion, resolving speed and resolving position of the strapdown inertial navigation system by using the initial alignment, the integrated navigation and the integrated calibration, so that a calculated navigation coordinate system and a real navigation coordinate system coincide as much as possible;
the working process of the attitude calculation algorithm module comprises the following steps:
1. coarse alignment; 2. updating the attitude quaternion; 3. calculating an attitude angle; 4. calculating an attitude angular rate; 5. transforming specific force coordinates; 6. updating the speed; 7. updating the position; 8. performing Kalman filtering calculation; 9. and calculating a rotation control command.
The method comprises the following steps: coarse alignment
Coarse alignment, i.e. finding the rough orientation cosine array
The basic idea of the scheme is to use an inertial coordinate system as a reference standard, initially fix two inertial coordinate systems, update the quaternion of angular motion of a carrier relative to the inertial coordinate systems, and solve a transformation matrix of the two solidified inertial coordinate systems by using the transformation relation between gravity and the linear motion of the carrier between the two inertial coordinate systems, thereby reversely deducing
The orientation cosine array is firstly decomposed into
The problems are converted into respective solutions
And
the method comprises the following steps:
1) to find
Will be provided with
Is written into
Wherein
λ0And L0To align the start time longitude and latitude, λtAnd LtTo align the longitude and latitude at time t, (4.1.3), (4.1.4), (4.1.5) and (4.1.6) are substituted into (4.1.2) to obtain
2) To find
The differential equation is obtained by utilizing the attitude quaternion updating as the attitude updating algorithm
Wherein
For gyro-sensitive carrier angular velocities, the alignment start time ib0 coincides with b,
3) to find
Under the inertial system
Accelerometer output under moving base
gbIs a projection of the gravity on the b system,
acceleration of linear motion of carrier
Definition of
Is provided with
Under the condition that the carrier does uniform motion or equal-amplitude swinging motion, the motion is approximate
Is provided with
Wherein,
determined from step 1) gn＝[0 0 g]T
Further by basic equations of speed
Is written into
Then
Wherein v isbFor external reference to ground speed, define
Arranged in an upper formula
Wherein,
arbitrarily taking two time points in the alignment process
To obtain
Will be described in detail
And
the orientation cosine array can be obtained by substituting in formula (4.1)
From the above derivation, it can be seen that the largest error source in alignment is the acceleration of the carrier motion during alignment
For a two-axis stable platform
Relative to gbThe alignment device is small in size, can finish high-precision alignment only by requiring small maneuvering of the carrier in a short time even in the process of traveling, and has strong dynamic applicability.
Step two: attitude quaternion update
The transformation relation from n system to p system can be expressed by rotating quaternion Q, and quaternion is composed of a real number unit and 3 imaginary number units
A constituent supercomplex comprising 4 real elements, of the form:
wherein q0, q1, q2, q3 are four real numbers,
an orthogonal basis in imaginary units;
if the vector in the space is fixed, if the moving coordinate system rotates by an angle relative to the reference system, wherein the moving coordinate system is a p system, the reference coordinate system is an n system, and the transformation relation of the vector described by the quaternion on the two coordinate systems is
In the formula,
substituting (4.2.3) into (4.2.2), and calculating according to quaternion to obtain
And is composed of
Then the transformation matrix can be obtained
The above being obtained by quaternion
If the posture quaternion Q from n system to p system is known, the corresponding transformation array can be obtained;
the rotation coordinate system at the time tk is p (k), the navigation coordinate system is n (k), the carrier coordinate system at the time tk +1 is p (k +1), the navigation coordinate system is n (k +1), the rotation quaternions from p (k) to p (k +1) are Q (h), the rotation quaternions from n (k) to b (k) are Q (tk), the rotation quaternions from n (k +1) to b (k +1) are Q (tk +1), the rotation quaternions from n (k) to n (k +1) are p (h), wherein h ═ tk +1-tk is an updating period, then the navigation coordinate system is n (k), the carrier coordinate system at the time tk +1 is p (k +1), the navigation coordinate system at the time tk +1 is n (k), the carrier coordinate system at the time tk +1 is n (k +1), the navigation coordinate system at the navigation coordinate system is n (k), the rotation quaternion from p (k) to p (k +1), the rotation quaternion from n (k) to b (k) is Q (tk), the rotation quaternion from n (tk) is Q (tk), the rotation quaternion from n (tk) is p (tk), the rotation quaternion (tk) from n (tk) is p (tk) and k), wherein the rotation quaternion is Q (tk) from n (tk) is an updating period, wherein the rotation quaternion of n (tk) is an updating period, and b (tk) of n (tk) of the updating period, wherein the updating period, and the updating period of n (tk) of the updating period of the updating
According to formula (4.2.2) and
the equivalence of the expression (4.2.7) is obtained by utilizing the equivalence relation between (4.2.2) and (4.2.5)
Comparing (4.2.8) and (4.2.9) to obtain
Wherein
Phi is an equivalent rotation vector from b (k) to b (k +1), and the numerical solution is obtained by the following equivalent rotation vector differential equation
Wherein Φ (h) is an equivalent rotation vector from b (k) to b (k +1) in an attitude updating period (tk, tk +1), ω is a carrier angular rate vector, and the latter two terms reflect rotation irreplaceable error compensation;
the equation (4.2.12) can be solved by a Taylor series expansion method, polynomial fitting is carried out on the angular velocity of the carrier in an attitude updating period (tk, tk +1), multi-subsample sampling is carried out on the gyro signal by a sampling period shorter than the updating period, the coefficient of the fitting polynomial is solved by utilizing the angular increment information of each sampling point, and the fitted attitude angular velocity is replaced into the Taylor series expansion formula of phi (h) to obtain the numerical solution of the rotation vector;
if the carrier does rolling and pitching vibration with same frequency and different phases, a cone error is generated on an azimuth axis, when the vibration meets a certain phase and frequency relation, the vibration of any two axes causes drift in a third axial direction, and the cone error is compensated by adopting a rotating vector quadrilaterals optimization algorithm:
wherein, Delta theta1、Δθ2、Δθ3、Δθ4Are respectively (tk, tk + h/4)]、(tk+h/4,tk+h/2]、(tk+h/2,tk+3h/4]、(tk+3h/4,tk+1]A gyro output angle increment within a sampling period;
in addition, n is the angle rotated during the update cycle relative to i
the velocity, latitude and curvature radius in the formula are all the results of the last navigation solution update period, and
within one pose update period, the rotation of the navigational coordinate system changes very slowly,
for the minimum, it is not advisable to use dividends in computers, and taylor expansion is performed on equation (4.2.17) to get a fourth order approximation:
the normalization process was performed after substituting (4.2.11) and (4.2.18) into (4.2.10)
Q(tk+1)＝Q(tk+1)/||Q(tk+1)|| (4.2.19)
Step three: attitude angle calculation
The above formula result is substituted into formula (4.2.6), and the attitude transformation array of the inertia measurement assembly can be calculated
Inertia measurement assembly of single-axis rotation laser inertial navigation around rotation axis Ozp1Rotation, assuming the rotation angle position is gamma, the attitude transformation matrix from p1 to b0 is
Attitude transformation array from n series to p1 series
Is composed of
After the rotation angle transformation, the posture transformation array from n system to b0 system
Is composed of
The general expression of the attitude transformation array of the inertial navigation system is
Attitude transformation array capable of calculating ship carrier
Wherein,
the attitude zero array is obtained by calibrating the equipment on a ship with a true level and a true course;
finally, the horizontal attitude angle and the course angle of the carrier are extracted in real time by comparing the formula (3.2.4) with the formula (4.3.5)
The initial value of quaternion is calculated by the initial attitude angle
Wherein the initial values of attitude angles H0, P0, R0 are determined by the initial alignment;
step four: attitude angular rate calculation
Taking N samples of the smoothed quantity according to a sampling period h (h ═ tk-tk-1), and enabling the N samples to be smoothed
Reissue to order
A＝(BTB)-1BT (4.4.2)
Definition of
A(2,:)＝[A(2,1) A(2,2) ... A(2,N)] (4.4.3)
A (2) represents the second row elements of the matrix A, the number of the elements is N, and the elements can be stored for later use after being pre-calculated;
attitude angular rate at current time tk
And
is calculated as follows
Wherein R (t)k+i-N)、P(tk+i-N) And H (t)k+i-N) The roll angle, the pitch angle and the course angle at the time tk + i-N;
step five: transformation of specific force coordinates
Accelerometer output specific force fpVia coordinate transformation
Accelerometers mounted on the axis of rotation of the rotary mechanism with negligible inner lever arm effect, i.e.
Step six: speed update
Basic equation of inertial navigation
Wherein,
is the projection of the geographic coordinate system relative to the earth coordinate system in the geographic coordinate system, called ground speed. The writing of equation (4.6.1) into components is in the form of:
namely, it is
Wherein, note
δ a is called detrimental acceleration;
then
Within one speed update period (tk, tk +1), there is
The accelerometer outputs pulses as velocity increments, and the integral term in equation (4.43) is calculated from
Wherein, will
Is marked as
ΔvpOutputting the speed increment of the accelerometer after the pulse number is subjected to scale conversion, wherein h is an updating period;
first term on right side of formula (4.6.7)
The speed compensation quantity caused by specific force;
the formula (4.6.8) can generate constant errors related to the linear motion and angular motion of the carrier under the dynamic condition of the carrier, if the carrier does angular motion in the linear motion direction at the same time, a rotation effect can be generated, if the carrier does linear vibration along the longitudinal axis and simultaneously does angular vibration with same frequency and same phase along the transverse axis, direct current can be generated in the azimuth axis direction of the carrier, namely, a constant speed increment error is introduced, and the motion mode is similar to the rowing motion: on the one hand, the paddle is periodically rocked around the lateral axis of the boat, and on the other hand, the boat is intermittently advanced along the longitudinal axis, so that the paddle effect is called, and the compensation method for the rotation effect and the paddle effect is to delta vpSampling a plurality of subsamples, fitting a motion track in an updating period by using sampling point information, and determining the coefficient of a fitting polynomial by using an optimization goal that an error caused by an algorithm is as small as possible under the extreme condition of the rowing motion;
the rotation effect compensation term is a primary compensation term, an optimized four-subsample algorithm is adopted for the scratch effect compensation term, and the calculation formula is as follows
Δvp＝Δv+Δvrot+Δvscul (4.6.9)
Wherein the amount of compensation for the rotation effect
Optimized compensation quantity of four-subsample sculling effect
Δv＝Δv1+Δv2+Δv3+Δv4 (4.6.12)
Δθ＝Δθ1+Δθ2+Δθ3+Δθ4 (4.6.13)
Δv1、Δv2、Δv3、Δv4Are respectively (tk, tk + h/4)]、(tk+h/4,tk+h/2]、(tk+h/2,tk+3h/4]、(tk+3h/4,tk+1](ii) a velocity increment of the accelerometer output over a sampling period;
step seven: location update
In a pure inertia working mode, a height channel of an inertial navigation system is divergent, and for water surface application, a high-pass filtering mode is adopted to obtain instantaneous vertical speed and instantaneous displacement;
differential equation of longitude and latitude is as follows
Within one location update period (tk, tk +1), there is
Step eight: kalman filter calculation
The algorithm is as follows:
a) equation of state
Both gyroscope drift and accelerometer bias can be considered as random constant processes, including
State variable selection of 15 dimensions
The equation of state for a continuous system is:
from an inertial system error propagation equation, wherein:
b) equation of measurement
The measurement equation using the position as the observed quantity is
In the formula, subscript s represents resolving output of the strapdown inertial navigation system, subscript r represents reference output, Hv＝[03×6 I3×303×6]V is 3-dimensional position observation white noise;
c) correction
Kalman filtering estimates the misalignment angle and corrects the attitude array in real time
Then obtaining a relatively accurate attitude array;
misalignment angle phi estimated using Kalman filtering techniquesE、φN、φUThe correction angle is:
the quaternion method is adopted for correction as follows:
wherein:
the speed and position are corrected by replacing the speed and position filter output values;
d) discretization of continuous systems
The continuous system error model is established, discretization is needed in the program, and the discretization is essentially to calculate a transfer matrix of a discrete system according to a system matrix of the continuous system and calculate a noise variance matrix of the discrete system according to a system process variance intensity matrix of the continuous system;
the equation of state model for a continuous system is as follows:
wherein X is an n-dimensional state vector of the system, F is an n × n-dimensional system matrix, W is a p-dimensional system process noise, G is an n × p-dimensional noise input matrix, Z is an m-dimensional observation vector of the system, V is an m-dimensional observation noise, and H is an m × n-dimensional observation matrix;
1)Φk,k-1is calculated by
Calculation method using a steady state system, state transition matrix Φk,k-1The system matrix F is related by:
wherein, (tk-1, tk ] is a prediction period, and it is noted that h ═ tk-tk-1. the prediction period h is generally short, and taylor expansion is performed on the formula (4.9.18), to obtain
2)
Is calculated by
The covariance matrix of the system noise vectors w (t) of the continuous system is q (t), and the covariance matrix of the input noise is:
Qq＝G(t)Q(t)GT(t) (formula-20)
Kalman filtered input noise variance
Input noise variance with continuous system QqThe following relationships exist:
step nine: rotation control target angular position calculation
The target of the stable platform is to isolate the horizontal angular motion of the carrier, and for the control, the constraint conditions are that ═ R ^ (R + α) dt ═ 0, - (P + β) dt ═ 0, where R is the roll angle, P is the pitch angle, α is the roll angle instruction, β is the pitch angle instruction, i.e., the rotation must have the function of tracking the change of the horizontal attitude angle;
the output command angle is forwarded to a control algorithm for calculation and conversion into a control quantity (or a control signal).
Advantageous effects
Compared with the prior art, the invention has the following advantages.
The invention provides an attitude measurement method based on inertia/satellite navigation (INS/GPS) combination, which avoids measurement errors caused by filter delay and accelerometer interference through algorithm design, and can ensure that a biaxial stable platform can still keep stable accuracy of a longitudinal rocking angle and a transverse rocking angle less than or equal to 10 ″ (RMS) under the load condition of loading an 80kg gravimeter and the large dynamic condition of a four-level sea state.
Drawings
The invention is further illustrated by the following figures and examples.
FIG. 1 is a schematic diagram of attitude resolution for a two-axis stabilized platform;
FIG. 2 is a coarse alignment process based on an inertial solidification coordinate system;
FIG. 3 is a schematic block diagram of an attitude resolving Kalman filtering algorithm;
FIG. 4 is vehicle Pitch (Pitch) and Roll (Roll) data;
FIG. 5 is vehicle Heading data;
FIG. 6 is Pitch (Pitch) and Roll (Roll) data for vehicle maneuvers;
FIG. 7 is Heading data under vehicular maneuvers;
FIG. 8 is data of pitch and roll angles for marine testing
FIG. 9 is a profile view of a bi-axial stabilization platform;
FIG. 10 is a diagram of inertial component hardware relationships.
Detailed Description
A dynamic attitude measurement method of a biaxial stable platform adopts Kalman filtering technology to carry out initial alignment, integrated navigation and integrated calibration of a strapdown inertial navigation system, estimates out misalignment angles, speed errors and position errors, and corrects quaternion, resolving speed and resolving position of the strapdown inertial navigation system by using the misalignment angles, the speed errors and the position errors so as to enable a calculated navigation coordinate system to coincide with a real navigation coordinate system as much as possible.
The working process of the attitude calculation algorithm module comprises the following steps:
1. coarse alignment; 2. updating the attitude quaternion; 3. calculating an attitude angle; 4. calculating an attitude angular rate; 5. transforming specific force coordinates; 6. updating the speed; 7. updating the position; 8. performing Kalman filtering calculation; 9. calculating a rotation control instruction;
the method comprises the following steps: coarse alignment
Coarse alignment, i.e. finding the rough orientation cosine array
The basic idea of the scheme is to use an inertial coordinate system as a reference standard, initially fix two inertial coordinate systems, update the quaternion of angular motion of a carrier relative to the inertial coordinate systems, and solve a transformation matrix of the two solidified inertial coordinate systems by using the transformation relation between gravity and the linear motion of the carrier between the two inertial coordinate systems, thereby reversely deducing
The orientation cosine array is firstly decomposed into
The problems are converted into respective solutions
And
the method comprises the following steps:
4) to find
Will be provided with
Is written into
Wherein
λ0And L0To align the start time longitude and latitude, λtAnd LtTo align the longitude and latitude at time t, (4.1.3), (4.1.4), (4.1.5) and (4.1.6) are substituted into (4.1.2) to obtain
5) To find
The differential equation is obtained by utilizing the attitude quaternion updating as the attitude updating algorithm
Wherein
For gyro-sensitive carrier angular velocities, the alignment start time ib0 coincides with b,
6) to find
Under the inertial system
Accelerometer output under moving base
gbIs a projection of the gravity on the b system,
as a carrier wireAcceleration of motion
Definition of
Is provided with
Under the condition that the carrier does uniform motion or equal-amplitude swinging motion, the motion is approximate
Is provided with
Wherein,
determined from step 1) gn＝[0 0 g]T
Further by basic equations of speed
Is written into
Then
Wherein v isbFor external reference to ground speed, define
Arranged in an upper formula
Wherein,
arbitrarily taking two time points in the alignment process
To obtain
Will be described in detail
And
the orientation cosine array can be obtained by substituting in formula (4.1)
From the above derivation, it can be seen that the largest error source in alignment is the acceleration of the carrier motion during alignment
For a two-axis stable platform
Relative to gbThe alignment device is small in amount, can finish high-precision alignment only by requiring small maneuvering of the carrier in a short time even in the process of traveling, and has strong dynamic applicability;
step two: attitude quaternion update
The transformation relation from n system to p system can be expressed by rotating quaternion Q, and quaternion is composed of a real number unit and 3 imaginary number units
A constituent supercomplex comprising 4 real elements, of the form:
wherein q0, q1, q2, q3 are four real numbers,
an orthogonal basis in imaginary units;
if the vector in the space is fixed, if the moving coordinate system rotates by an angle relative to the reference system, wherein the moving coordinate system is a p system, the reference coordinate system is an n system, and the transformation relation of the vector described by the quaternion on the two coordinate systems is
In the formula,
substituting (4.2.3) into (4.2.2), and calculating according to quaternion to obtain
And is composed of
Then the transformation matrix can be obtained
The above being obtained by quaternion
If the posture quaternion Q from n system to p system is known, the corresponding transformation array can be obtained;
the rotation coordinate system at the time tk is p (k), the navigation coordinate system is n (k), the carrier coordinate system at the time tk +1 is p (k +1), the navigation coordinate system is n (k +1), the rotation quaternions from p (k) to p (k +1) are Q (h), the rotation quaternions from n (k) to b (k) are Q (tk), the rotation quaternions from n (k +1) to b (k +1) are Q (tk +1), the rotation quaternions from n (k) to n (k +1) are p (h), wherein h ═ tk +1-tk is an updating period, then the navigation coordinate system is n (k), the carrier coordinate system at the time tk +1 is p (k +1), the navigation coordinate system at the time tk +1 is n (k), the carrier coordinate system at the time tk +1 is n (k +1), the navigation coordinate system at the navigation coordinate system is n (k), the rotation quaternion from p (k) to p (k +1), the rotation quaternion from n (k) to b (k) is Q (tk), the rotation quaternion from n (tk) is Q (tk), the rotation quaternion from n (tk) is p (tk), the rotation quaternion (tk) from n (tk) is p (tk) and k), wherein the rotation quaternion is Q (tk) from n (tk) is an updating period, wherein the rotation quaternion of n (tk) is an updating period, and b (tk) of n (tk) of the updating period, wherein the updating period, and the updating period of n (tk) of the updating period of the updating
According to formula (4.2.2) and
the equivalence of the expression (4.2.7) is obtained by utilizing the equivalence relation between (4.2.2) and (4.2.5)
Comparing (4.2.8) and (4.2.9) to obtain
Wherein
Phi is an equivalent rotation vector from b (k) to b (k +1), and the numerical solution is obtained by the following equivalent rotation vector differential equation
Wherein Φ (h) is an equivalent rotation vector from b (k) to b (k +1) in an attitude updating period (tk, tk +1), ω is a carrier angular rate vector, and the latter two terms reflect rotation irreplaceable error compensation;
the equation (4.2.12) can be solved by a Taylor series expansion method, polynomial fitting is carried out on the angular velocity of the carrier in an attitude updating period (tk, tk +1), multi-subsample sampling is carried out on the gyro signal by a sampling period shorter than the updating period, the coefficient of the fitting polynomial is solved by utilizing the angular increment information of each sampling point, and the fitted attitude angular velocity is replaced into the Taylor series expansion formula of phi (h) to obtain the numerical solution of the rotation vector;
if the carrier does rolling and pitching vibration with same frequency and different phases, a cone error is generated on an azimuth axis, when the vibration meets a certain phase and frequency relation, the vibration of any two axes causes drift in a third axial direction, and the cone error is compensated by adopting a rotating vector quadrilaterals optimization algorithm:
wherein, Delta theta1、Δθ2、Δθ3、Δθ4Are respectively (tk, tk + h/4)]、(tk+h/4,tk+h/2]、(tk+h/2,tk+3h/4]、(tk+3h/4,tk+1]A gyro output angle increment within a sampling period;
in addition, n is the angle rotated during the update cycle relative to i
the velocity, latitude and curvature radius in the formula are all the results of the last navigation solution update period, and
within one pose update period, the rotation of the navigational coordinate system changes very slowly,
for the minimum, it is not advisable to use dividends in computers, and taylor expansion is performed on equation (4.2.17) to get a fourth order approximation:
the normalization process was performed after substituting (4.2.11) and (4.2.18) into (4.2.10)
Q(tk+1)＝Q(tk+1)/||Q(tk+1)|| (4.2.19)
Step three: attitude angle calculation
The upper typeThe result is substituted (4.2.6), and the attitude transformation matrix of the inertial measurement unit can be calculated
Inertia measurement assembly of single-axis rotation laser inertial navigation around rotation axis Ozp1Rotation, assuming the rotation angle position is gamma, the attitude transformation matrix from p1 to b0 is
Attitude transformation array from n series to p1 series
Is composed of
After the rotation angle transformation, the posture transformation array from n system to b0 system
Is composed of
The general expression of the attitude transformation array of the inertial navigation system is
Attitude transformation array capable of calculating ship carrier
Wherein,
for equipment on shipsObtaining an attitude zero position array after the true level and the true course calibration;
finally, the horizontal attitude angle and the course angle of the carrier are extracted in real time by comparing the formula (3.2.4) with the formula (4.3.5)
The initial value of quaternion is calculated by the initial attitude angle
Wherein the initial values of attitude angles H0, P0, R0 are determined by the initial alignment;
step four: attitude angular rate calculation
Taking N samples of the smoothed quantity according to a sampling period h (h ═ tk-tk-1), and enabling the N samples to be smoothed
Reissue to order
A＝(BTB)-1BT (4.4.2)
Definition of
A(2,:)＝[A(2,1) A(2,2) ... A(2,N)] (4.4.3)
A (2) represents the second row elements of the matrix A, the number of the elements is N, and the elements can be stored for later use after being pre-calculated;
attitude angular rate at current time tk
And
is calculated as follows
Wherein R (t)k+i-N)、P(tk+i-N) And H (t)k+i-N) The roll angle, the pitch angle and the course angle at the time tk + i-N;
step five: transformation of specific force coordinates
Accelerometer output specific force fpVia coordinate transformation
Accelerometers mounted on the axis of rotation of the rotary mechanism with negligible inner lever arm effect, i.e.
Step six: speed update
Basic equation of inertial navigation
Wherein,
for the projection of the geographic coordinate system relative to the earth coordinate system in the geographic coordinate system, called ground speed, equation (4.6.1) is written in the form of components:
namely, it is
Wherein, note
δ a is called detrimental acceleration;
then
Within one speed update period (tk, tk +1), there is
The accelerometer outputs pulses as velocity increments, and the integral term in equation (4.43) is calculated from
Wherein, will
Is marked as
ΔvpOutputting the speed increment of the accelerometer after the pulse number is subjected to scale conversion, wherein h is an updating period;
first term on right side of formula (4.6.7)
The speed compensation quantity caused by specific force;
equation (4.6.8) produces constant errors associated with linear and angular motion of the carrier under dynamic conditions of the carrierIf the carrier does linear vibration along the longitudinal axis and simultaneously does angular vibration along the transverse axis with same frequency and phase, the direction axis of the carrier generates direct current, namely, a constant speed increment error is introduced, and the motion mode is similar to the rowing motion: on the one hand, the paddle is periodically rocked around the lateral axis of the boat, and on the other hand, the boat is intermittently advanced along the longitudinal axis, so that the paddle effect is called, and the compensation method for the rotation effect and the paddle effect is to delta vpSampling a plurality of subsamples, fitting a motion track in an updating period by using sampling point information, and determining the coefficient of a fitting polynomial by using an optimization goal that an error caused by an algorithm is as small as possible under the extreme condition of the rowing motion;
the rotation effect compensation term is a primary compensation term, an optimized four-subsample algorithm is adopted for the scratch effect compensation term, and the calculation formula is as follows
Δvp＝Δv+Δvrot+Δvscul (4.6.9)
Wherein the amount of compensation for the rotation effect
Optimized compensation quantity of four-subsample sculling effect
Δv＝Δv1+Δv2+Δv3+Δv4 (4.6.12)
Δθ＝Δθ1+Δθ2+Δθ3+Δθ4 (4.6.13)
Δv1、Δv2、Δv3、Δv4Are respectively (tk, tk + h/4)]、(tk+h/4,tk+h/2]、(tk+h/2,tk+3h/4]、(tk+3h/4,tk+1](ii) a velocity increment of the accelerometer output over a sampling period;
step seven: location update
In a pure inertia working mode, a height channel of an inertial navigation system is divergent, and for water surface application, a high-pass filtering mode is adopted to obtain instantaneous vertical speed and instantaneous displacement;
differential equation of longitude and latitude is as follows
Within one location update period (tk, tk +1), there is
Step eight: kalman filter calculation
The algorithm is as follows:
e) equation of state
Both gyroscope drift and accelerometer bias can be considered as random constant processes, including
State variable selection of 15 dimensions
The equation of state for a continuous system is:
from an inertial system error propagation equation, wherein:
f) equation of measurement
The measurement equation using the position as the observed quantity is
In the formula, subscript s represents resolving output of the strapdown inertial navigation system, subscript r represents reference output, Hv＝[03×6 I3×303×6]V is 3-dimensional position observation white noise;
g) correction
Kalman filtering estimates the misalignment angle and corrects the attitude array in real time
Then obtaining a relatively accurate attitude array;
misalignment angle phi estimated using Kalman filtering techniquesE、φN、φUThe correction angle is:
the quaternion method is adopted for correction as follows:
wherein:
the speed and position are corrected by replacing the speed and position filter output values;
h) discretization of continuous systems
The continuous system error model is established, discretization is needed in the program, and the discretization is essentially to calculate a transfer matrix of a discrete system according to a system matrix of the continuous system and calculate a noise variance matrix of the discrete system according to a system process variance intensity matrix of the continuous system;
the equation of state model for a continuous system is as follows:
wherein X is an n-dimensional state vector of the system, F is an n × n-dimensional system matrix, W is a p-dimensional system process noise, G is an n × p-dimensional noise input matrix, Z is an m-dimensional observation vector of the system, V is an m-dimensional observation noise, and H is an m × n-dimensional observation matrix;
3)Φk,k-1is calculated by
Calculation method using a steady state system, state transition matrix Φk,k-1The system matrix F is related by:
wherein, (tk-1, tk ] is a prediction period, and it is noted that h ═ tk-tk-1. the prediction period h is generally short, and taylor expansion is performed on the formula (4.9.18), to obtain
4)
Is calculated by
The covariance matrix of the system noise vectors w (t) of the continuous system is q (t), and the covariance matrix of the input noise is:
Qq＝G(t)Q(t)GT(t) (formula-20)
Kalman filtered input noise variance
Input noise variance with continuous system QqThe following relationships exist:
step nine: rotation control target angular position calculation
The target of the stable platform is to isolate the horizontal angular motion of the carrier, and for the control, the constraint conditions are that ═ R ^ (R + α) dt ═ 0, - (P + β) dt ═ 0, where R is the roll angle, P is the pitch angle, α is the roll angle instruction, β is the pitch angle instruction, i.e., the rotation must have the function of tracking the change of the horizontal attitude angle;
the output command angle is forwarded to a control algorithm for calculation and conversion into a control quantity (or a control signal).
FIG. 1 is a schematic diagram of attitude calculation of a biaxial stabilized platform, which covers all the processes of steps 1-9, and in which the main data flow and data processing performed at each stage are drawn, and the schematic diagram and the algorithm description in the description are combined to make it possible to implement the method;
FIG. 2 is a coarse alignment procedure based on the inertial coagulation coordinate system, which is well known to those skilled in the strapdown inertial navigation art;
FIG. 3 is a schematic block diagram of an attitude solution Kalman filter algorithm, which is well known to those skilled in the art, but whose configuration parameters are unique to the present invention;
FIGS. 4-8 show land vehicle test data from which we can see a pitch error RMS value of 0.0002 (0.72 ') and a roll error RMS value of 0.002 (7.2') under static conditions, and from which we can see a pitch error RMS value of 0.0026 (9.36 ') and a roll error RMS value of 0.0025 (9') under dynamic conditions. From the heading change states of FIGS. 8 and 10, it can be seen that there is a significant difference between static and dynamic;
the data curve of fig. 8 shows that the maximum value of the data does not exceed 0.01 ° (3.6 ") for a continuous operating time of nearly 5 days at sea. The straight part of the curve in the figure is the period of time during which the vessel is moored, and the up-and-down fluctuation of the curve is the period of time during which the vessel is motored. Therefore, the stability of the double-shaft stable platform using the control method under a large dynamic environment is greatly improved, and the stable work of the gravimeter in various motion environments is powerfully guaranteed;
fig. 9 is a structural diagram of an external shape of a biaxial stable platform, in which the algorithm proposed by the present technical solution is implemented in a control box. The code in the figure shows the following components: 1. a cold atom gravimeter; 2. a base; 3. an outer frame; 4. an inertial component; 5. a control box; 6. a display; 7. a locking device. The middle frame is not identified in the drawing due to view occlusion;
the algorithm used in the present solution runs on the acquisition board in the inertial component, and the hardware configuration of the inertial component is shown in fig. 10.
The inertia measurement component mainly comprises an inertia sensor, an electronic circuit unit (EU), a platform body component and a case;
the inertia sensitive device comprises three optical fiber gyroscopes and three quartz accelerometers;
the electronic circuit unit comprises an accelerometer signal conversion board, an acquisition and resolving board and a direct current power supply module.
The accelerometer signal conversion board converts current signals of the three accelerometers into digital pulse signals, measures temperature measurement signals output by temperature measurement devices of the accelerometers, and outputs the signals to the acquisition and calculation board;
the acquisition resolving board generates a 1ms external trigger signal to the fiber optic gyroscope and accelerometer circuit board, acquires a gyroscope pulse signal, a temperature signal, an accelerometer pulse signal and a temperature signal through a 422 serial port, performs temperature compensation and scale transformation to obtain a carrier angular velocity and a specific force, and performs Kalman filtering calculation and strapdown algorithm navigation resolving; the acquisition resolving board receives external satellite guidance reference information at the same time, outputs a navigation resolving result to a user through an Ethernet/CAN bus/RS-422 asynchronous serial port/optical fiber interface, and interacts with the user through a display control device;
the DCDC direct-current power supply module converts the +24V direct-current power supply converted by the power supply device into various direct-current power supplies required by the operation of the gyroscope, the accelerometer circuit board, the acquisition and calculation board and the interface storage board.
The inertia assembly is arranged at the lower part of the cold atom gravimeter and is arranged on the stable frame in the middle of the double-shaft stable platform together with the cold atom gravimeter, and the inertia assembly and the cold atom gravimeter move synchronously. When the whole set of equipment is arranged on a vehicle, a ship and other motion carriers to move, the cold atom gravimeter needs to be kept in a horizontal state, namely the pitch angle and the roll angle between the working plane of the platform and the horizontal plane meet the requirement that the pitch angle and the roll angle are less than or equal to 10 ″. The inertia assembly is an important component for measuring the two key parameters, and the algorithm proposed by the technical scheme is an important method for calculating the two parameters.
Claims (1)
1. A dynamic attitude measurement method of a biaxial stable platform is characterized in that a Kalman filtering technology is adopted to carry out initial alignment, integrated navigation and integrated calibration of a strapdown inertial navigation system, a misalignment angle, a speed error and a position error are estimated, and a quaternion, a resolving speed and a resolving position of the strapdown inertial navigation system are corrected by using the initial alignment, the integrated navigation and the integrated calibration, so that a calculated navigation coordinate system and a real navigation coordinate system coincide as much as possible;
the attitude calculation algorithm module workflow comprises the following steps:
1. coarse alignment; 2. updating the attitude quaternion; 3. calculating an attitude angle; 4. calculating an attitude angular rate; 5. transforming specific force coordinates; 6. updating the speed; 7. updating the position; 8. performing Kalman filtering calculation; 9. calculating a rotation control instruction;
the method comprises the following steps: coarse alignment
Coarse alignment, i.e. finding the rough orientation cosine array
The basic idea of the scheme is to use an inertial coordinate system as a reference standard, initially fix two inertial coordinate systems, update the quaternion of angular motion of a carrier relative to the inertial coordinate systems, and solve a transformation matrix of the two solidified inertial coordinate systems by using the transformation relation between gravity and the linear motion of the carrier between the two inertial coordinate systems, thereby reversely deducing
The orientation cosine array is firstly decomposed into
The problems are converted into respective solutions
And
the method comprises the following steps:
to find
Will be provided with
Is written into
Wherein
λ0And L0To align the start time longitude and latitude, λtAnd LtTo align the longitude and latitude at time t, (4.1.3), (4.1.4), (4.1.5) and (4.1.6) are substituted into (4.1.2) to obtain
To find
The differential equation is obtained by utilizing the attitude quaternion updating as the attitude updating algorithm
Wherein
For gyro-sensitive carrier angular velocities, the alignment start time ib0 coincides with b,
to find
Under the inertial system
Accelerometer output under moving base
gbIs a projection of the gravity on the b system,
acceleration of linear motion of carrier
Definition of
Is provided with
Under the condition that the carrier does uniform motion or equal-amplitude swinging motion, the motion is approximate
Is provided with
Wherein,
determined from step 1) gn＝[0 0 g]T
Further by basic equations of speed
Is written into
Then
Wherein v isbFor external reference to ground speed, define
Arranged in an upper formula
Wherein,
arbitrarily taking two time points in the alignment process
To obtain
Will be described in detail
And
the orientation cosine array can be obtained by substituting in formula (4.1)
From the above derivation, it can be seen that the largest error source in alignment is the acceleration of the carrier motion during alignment
For a two-axis stable platform
Relative to gbIn small quantities, even inThe alignment with higher precision can be completed only by requiring less maneuvering of the carrier in a short time during traveling, and the dynamic applicability is stronger;
step two: attitude quaternion update
The transformation relation from n system to p system can be expressed by rotating quaternion Q, and quaternion is composed of a real number unit and 3 imaginary number units
A constituent supercomplex comprising 4 real elements, of the form:
wherein q0, q1, q2, q3 are four real numbers,
an orthogonal basis in imaginary units;
if the vector in the space is fixed, if the moving coordinate system rotates by an angle relative to the reference system, wherein the moving coordinate system is a p system, the reference coordinate system is an n system, and the transformation relation of the vector described by the quaternion on the two coordinate systems is
In the formula,
substituting (4.2.3) into (4.2.2), and calculating according to quaternion to obtain
And is composed of
Then the transformation matrix can be obtained
The above being obtained by quaternion
If the posture quaternion Q from n system to p system is known, the corresponding transformation array can be obtained;
the rotation coordinate system at the time tk is p (k), the navigation coordinate system is n (k), the carrier coordinate system at the time tk +1 is p (k +1), the navigation coordinate system is n (k +1), the rotation quaternions from p (k) to p (k +1) are Q (h), the rotation quaternions from n (k) to b (k) are Q (tk), the rotation quaternions from n (k +1) to b (k +1) are Q (tk +1), the rotation quaternions from n (k) to n (k +1) are p (h), wherein h ═ tk +1-tk is an updating period, then the navigation coordinate system is n (k), the carrier coordinate system at the time tk +1 is p (k +1), the navigation coordinate system at the time tk +1 is n (k), the carrier coordinate system at the time tk +1 is n (k +1), the navigation coordinate system at the navigation coordinate system is n (k), the rotation quaternion from p (k) to p (k +1), the rotation quaternion from n (k) to b (k) is Q (tk), the rotation quaternion from n (tk) is Q (tk), the rotation quaternion from n (tk) is p (tk), the rotation quaternion (tk) from n (tk) is p (tk) and k), wherein the rotation quaternion is Q (tk) from n (tk) is an updating period, wherein the rotation quaternion of n (tk) is an updating period, and b (tk) of n (tk) of the updating period, wherein the updating period, and the updating period of n (tk) of the updating period of the updating
According to formula (4.2.2) and
the equivalence of the expression (4.2.7) is obtained by utilizing the equivalence relation between (4.2.2) and (4.2.5)
Comparing (4.2.8) and (4.2.9) to obtain
Wherein
Phi is an equivalent rotation vector from b (k) to b (k +1), and the numerical solution is obtained by the following equivalent rotation vector differential equation
Wherein Φ (h) is an equivalent rotation vector from b (k) to b (k +1) in an attitude updating period (tk, tk +1), ω is a carrier angular rate vector, and the latter two terms reflect rotation irreplaceable error compensation;
the equation (4.2.12) can be solved by a Taylor series expansion method, polynomial fitting is carried out on the angular velocity of the carrier in an attitude updating period (tk, tk +1), multi-subsample sampling is carried out on the gyro signal by a sampling period shorter than the updating period, the coefficient of the fitting polynomial is solved by utilizing the angular increment information of each sampling point, and the fitted attitude angular velocity is replaced into the Taylor series expansion formula of phi (h) to obtain the numerical solution of the rotation vector;
if the carrier does rolling and pitching vibration with same frequency and different phases, a cone error is generated on an azimuth axis, when the vibration meets a certain phase and frequency relation, the vibration of any two axes causes drift in a third axial direction, and the cone error is compensated by adopting a rotating vector quadrilaterals optimization algorithm:
wherein, Delta theta1、Δθ2、Δθ3、Δθ4Are respectively (tk, tk + h/4)]、(tk+h/4,tk+h/2]、(tk+h/2,tk+3h/4]、(tk+3h/4,tk+1]A gyro output angle increment within a sampling period;
in addition, n is the angle rotated during the update cycle relative to i
the velocity, latitude and curvature radius in the formula are all the results of the last navigation solution update period, and
within one pose update period, the rotation of the navigational coordinate system changes very slowly,
for the minimum, it is not advisable to use dividends in computers, and taylor expansion is performed on equation (4.2.17) to get a fourth order approximation:
the normalization process was performed after substituting (4.2.11) and (4.2.18) into (4.2.10)
Q(tk+1)＝Q(tk+1)/||Q(tk+1)|| (4.2.19)
Step three: attitude angle calculation
The above formula result is substituted into formula (4.2.6), and the attitude transformation array of the inertia measurement assembly can be calculated
Inertia measurement assembly of single-axis rotation laser inertial navigation around rotation axis Ozp1Rotation, assuming the rotation angle position is gamma, the attitude transformation matrix from p1 to b0 is
Attitude transformation array from n series to p1 series
Is composed of
After conversion of the rotation angle, n is to b0Attitude transformation array
Is composed of
The general expression of the attitude transformation array of the inertial navigation system is
Attitude transformation array capable of calculating ship carrier
Wherein,
the attitude zero array is obtained by calibrating the equipment on a ship with a true level and a true course;
finally, the horizontal attitude angle and the course angle of the carrier are extracted in real time by comparing the formula (3.2.4) with the formula (4.3.5)
The initial value of quaternion is calculated by the initial attitude angle
Wherein the initial values of attitude angles H0, P0, R0 are determined by the initial alignment; step four: attitude angular rate calculation
Taking N samples of the smoothed quantity according to a sampling period h (h ═ tk-tk-1), and enabling the N samples to be smoothed
Reissue to order
A＝(BTB)-1BT (4.4.2)
Definition of
A(2,:)＝[A(2,1) A(2,2) ... A(2,N)] (4.4.3)
A (2) represents the second row elements of the matrix A, the number of the elements is N, and the elements can be stored for later use after being pre-calculated;
attitude angular rate at current time tk
And
is calculated as follows
Wherein R (t)k+i-N)、P(tk+i-N) And H (t)k+i-N) The roll angle, the pitch angle and the course angle at the time tk + i-N;
step five: transformation of specific force coordinates
Accelerometer output specific force fpVia coordinate transformation
Accelerometers mounted on the axis of rotation of the rotary mechanism with negligible inner lever arm effect, i.e.
Step six: speed update
Basic equation of inertial navigation
Wherein,
is the projection of the geographic coordinate system relative to the earth coordinate system in the geographic coordinate system, called ground speed. The writing of equation (4.6.1) into components is in the form of:
namely, it is
Wherein, note
δ a is called detrimental acceleration;
then
Within one speed update period (tk, tk +1), there is
The accelerometer outputs pulses as velocity increments, and the integral term in equation (4.43) is calculated from
Wherein, will
Is marked as
ΔvpOutputting the speed increment of the accelerometer after the pulse number is subjected to scale conversion, wherein h is an updating period;
first term on right side of formula (4.6.7)
The speed compensation quantity caused by specific force;
the formula (4.6.8) can generate constant errors related to the linear motion and angular motion of the carrier under the dynamic condition of the carrier, if the carrier does angular motion in the linear motion direction at the same time, a rotation effect can be generated, if the carrier does linear vibration along the longitudinal axis and simultaneously does angular vibration with same frequency and same phase along the transverse axis, direct current can be generated in the azimuth axis direction of the carrier, namely, a constant speed increment error is introduced, and the motion mode is similar to the rowing motion: on the one hand, the paddle is periodically rocked around the lateral axis of the boat, and on the other hand, the boat is intermittently advanced along the longitudinal axis, so that the paddle effect is called, and the compensation method for the rotation effect and the paddle effect is to delta vpCarry out moreSub-sample sampling, fitting the motion track in the updating period by using the sample point information, and determining the coefficient of a fitting polynomial by using the optimization goal of minimizing the error caused by the algorithm under the extreme condition of the rowing motion;
the rotation effect compensation term is a primary compensation term, an optimized four-subsample algorithm is adopted for the scratch effect compensation term, and the calculation formula is as follows
Δvp＝Δv+Δvrot+Δvscul (4.6.9)
Wherein the amount of compensation for the rotation effect
Optimized compensation quantity of four-subsample sculling effect
Δv＝Δv1+Δv2+Δv3+Δv4 (4.6.12)
Δθ＝Δθ1+Δθ2+Δθ3+Δθ4 (4.6.13)
Δv1、Δv2、Δv3、Δv4Are respectively (tk, tk + h/4)]、(tk+h/4,tk+h/2]、(tk+h/2,tk+3h/4]、(tk+3h/4,tk+1](ii) a velocity increment of the accelerometer output over a sampling period;
step seven: location update
In a pure inertia working mode, a height channel of an inertial navigation system is divergent, and for water surface application, a high-pass filtering mode is adopted to obtain instantaneous vertical speed and instantaneous displacement;
differential equation of longitude and latitude is as follows
Within one location update period (tk, tk +1), there is
Step eight: kalman filter calculation
The algorithm is as follows:
equation of state
Both gyroscope drift and accelerometer bias can be considered as random constant processes, including
State variable selection of 15 dimensions
The equation of state for a continuous system is:
from an inertial system error propagation equation, wherein:
equation of measurement
The measurement equation using the position as the observed quantity is
In the formula, subscript s represents resolving output of the strapdown inertial navigation system, subscript r represents reference output, Hv＝[03×6 I3×303×6]V is 3-dimensional position observation white noise;
correction
Kalman filtering estimates the misalignment angle and corrects the attitude array in real time
Then obtaining a relatively accurate attitude array;
misalignment angle phi estimated using Kalman filtering techniquesE、φN、φUThe correction angle is:
the quaternion method is adopted for correction as follows:
wherein:
the speed and position are corrected by replacing the speed and position filter output values;
discretization of continuous systems
The continuous system error model is established, discretization is needed in the program, and the discretization is essentially to calculate a transfer matrix of a discrete system according to a system matrix of the continuous system and calculate a noise variance matrix of the discrete system according to a system process variance intensity matrix of the continuous system;
the equation of state model for a continuous system is as follows:
wherein X is an n-dimensional state vector of the system, F is an n × n-dimensional system matrix, W is a p-dimensional system process noise, G is an n × p-dimensional noise input matrix, Z is an m-dimensional observation vector of the system, V is an m-dimensional observation noise, and H is an m × n-dimensional observation matrix;
Φk,k-1is calculated by
Calculation method using a steady state system, state transition matrix Φk,k-1The system matrix F is related by:
wherein, (tk-1, tk ] is a prediction period, and it is noted that h ═ tk-tk-1. the prediction period h is generally short, and taylor expansion is performed on the formula (4.9.18), to obtain
The covariance matrix of the system noise vectors w (t) of the continuous system is q (t), and the covariance matrix of the input noise is:
Qq＝G(t)Q(t)GT(t) (formula-20)
Kalman filtered input noise variance
Input noise variance with continuous system QqThe following relationships exist:
step nine: rotation control target angular position calculation
The target of the stable platform is to isolate the horizontal angular motion of the carrier, and for the control, the constraint conditions are that ═ R ^ (R + α) dt ═ 0, - (P + β) dt ═ 0, where R is the roll angle, P is the pitch angle, α is the roll angle instruction, β is the pitch angle instruction, i.e., the rotation must have the function of tracking the change of the horizontal attitude angle;
the output command angle is forwarded to a control algorithm for calculation and is converted into a control quantity or a control signal.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111416781.9A CN114111771A (en) | 2021-11-25 | 2021-11-25 | Dynamic attitude measurement method of double-shaft stable platform |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111416781.9A CN114111771A (en) | 2021-11-25 | 2021-11-25 | Dynamic attitude measurement method of double-shaft stable platform |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114111771A true CN114111771A (en) | 2022-03-01 |
Family
ID=80373343
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111416781.9A Pending CN114111771A (en) | 2021-11-25 | 2021-11-25 | Dynamic attitude measurement method of double-shaft stable platform |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114111771A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114719812A (en) * | 2022-03-10 | 2022-07-08 | 同济大学 | Real-time line curvature detection system and method for active radial control |
| CN115371650A (en) * | 2022-08-23 | 2022-11-22 | 天津大学 | Six-degree-of-freedom laser target measuring system and dynamic performance improving method thereof |
Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CA2511000A1 (en) * | 2003-01-21 | 2004-07-21 | Novatel Inc. | Inertial gps navigation system with modified kalman filter |
| DE102017102269A1 (en) * | 2016-02-12 | 2017-08-17 | Gm Global Technology Operations, Llc | TILT AND MISSING EQUALIZATION FOR 6-DOF IMU USING GNSS / INS DATA |
| CN107525503A (en) * | 2017-08-23 | 2017-12-29 | 王伟 | Adaptive cascade kalman filter method based on double antenna GPS and MIMU combination |
| CN108180925A (en) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | A kind of odometer assists vehicle-mounted dynamic alignment method |
| CN108680186A (en) * | 2018-05-17 | 2018-10-19 | 中国人民解放军海军工程大学 | Methods of Strapdown Inertial Navigation System nonlinear initial alignment method based on gravimeter platform |
| CN110319833A (en) * | 2019-07-09 | 2019-10-11 | 哈尔滨工程大学 | A kind of error-free fiber-optic gyroscope strapdown inertial navigation system speed update method |
| CN110398257A (en) * | 2019-07-17 | 2019-11-01 | 哈尔滨工程大学 | The quick initial alignment on moving base method of SINS system of GPS auxiliary |
| AU2020101268A4 (en) * | 2020-07-06 | 2020-08-13 | Harbin Engineering University | The initial alignment method for sway base |
| CN112504275A (en) * | 2020-11-16 | 2021-03-16 | 哈尔滨工程大学 | Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm |
| CN112629538A (en) * | 2020-12-11 | 2021-04-09 | 哈尔滨工程大学 | Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering |
| CN112697141A (en) * | 2020-12-16 | 2021-04-23 | 北京航空航天大学 | Inertial navigation/odometer moving base posture and position alignment method based on reverse navigation |
| CN113281797A (en) * | 2021-05-11 | 2021-08-20 | 南京国睿防务系统有限公司 | Maneuvering detection and correction radar design based on inertial navigation |
| EP3896393A1 (en) * | 2020-04-17 | 2021-10-20 | Orolia USA Inc. | Methods for initializing a navigation system without heading information and devices thereof |
-
2021
- 2021-11-25 CN CN202111416781.9A patent/CN114111771A/en active Pending
Patent Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CA2511000A1 (en) * | 2003-01-21 | 2004-07-21 | Novatel Inc. | Inertial gps navigation system with modified kalman filter |
| DE102017102269A1 (en) * | 2016-02-12 | 2017-08-17 | Gm Global Technology Operations, Llc | TILT AND MISSING EQUALIZATION FOR 6-DOF IMU USING GNSS / INS DATA |
| CN107525503A (en) * | 2017-08-23 | 2017-12-29 | 王伟 | Adaptive cascade kalman filter method based on double antenna GPS and MIMU combination |
| CN108180925A (en) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | A kind of odometer assists vehicle-mounted dynamic alignment method |
| CN108680186A (en) * | 2018-05-17 | 2018-10-19 | 中国人民解放军海军工程大学 | Methods of Strapdown Inertial Navigation System nonlinear initial alignment method based on gravimeter platform |
| CN110319833A (en) * | 2019-07-09 | 2019-10-11 | 哈尔滨工程大学 | A kind of error-free fiber-optic gyroscope strapdown inertial navigation system speed update method |
| CN110398257A (en) * | 2019-07-17 | 2019-11-01 | 哈尔滨工程大学 | The quick initial alignment on moving base method of SINS system of GPS auxiliary |
| EP3896393A1 (en) * | 2020-04-17 | 2021-10-20 | Orolia USA Inc. | Methods for initializing a navigation system without heading information and devices thereof |
| AU2020101268A4 (en) * | 2020-07-06 | 2020-08-13 | Harbin Engineering University | The initial alignment method for sway base |
| CN112504275A (en) * | 2020-11-16 | 2021-03-16 | 哈尔滨工程大学 | Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm |
| CN112629538A (en) * | 2020-12-11 | 2021-04-09 | 哈尔滨工程大学 | Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering |
| CN112697141A (en) * | 2020-12-16 | 2021-04-23 | 北京航空航天大学 | Inertial navigation/odometer moving base posture and position alignment method based on reverse navigation |
| CN113281797A (en) * | 2021-05-11 | 2021-08-20 | 南京国睿防务系统有限公司 | Maneuvering detection and correction radar design based on inertial navigation |
Non-Patent Citations (4)
| Title |
|---|
| HIDE, C (HIDE, C) ; MOORE, T (MOORE, T) ; SMITH, M (SMITH, M): "Adaptive Kalman filtering for low-cost INS/GPS", JOURNAL OF NAVIGATION, vol. 56, no. 1, 1 January 2003 (2003-01-01), pages 143 - 152 * |
| 冯爱国;徐晓苏;: "GPS辅助姿态计算的捷联陀螺罗经实现", 上海海事大学学报, vol. 31, no. 04, 15 December 2010 (2010-12-15), pages 17 - 22 * |
| 徐景硕;罗恬颖;马士国;: "动基座条件下舰载机快速传递对准方法研究", 宇航计测技术, vol. 34, no. 06, 15 December 2014 (2014-12-15), pages 13 - 18 * |
| 薛亮;姜澄宇;常洪龙;袁广民;苑伟政;: "基于状态约束的MIMU/磁强计组合姿态估计滤波算法", 中国惯性技术学报, vol. 17, no. 03, 15 June 2009 (2009-06-15), pages 338 - 343 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114719812A (en) * | 2022-03-10 | 2022-07-08 | 同济大学 | Real-time line curvature detection system and method for active radial control |
| CN114719812B (en) * | 2022-03-10 | 2023-08-29 | 同济大学 | Line curvature real-time detection system and method for active radial control |
| CN115371650A (en) * | 2022-08-23 | 2022-11-22 | 天津大学 | Six-degree-of-freedom laser target measuring system and dynamic performance improving method thereof |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109813311B (en) | Unmanned aerial vehicle formation collaborative navigation method | |
| CN101413800B (en) | Navigating and steady aiming method of navigation / steady aiming integrated system | |
| EP1582840B1 (en) | Inertial navigation system error correction | |
| CN108827310B (en) | Marine star sensor auxiliary gyroscope online calibration method | |
| CN108051866B (en) | Based on strap down inertial navigation/GPS combination subsidiary level angular movement isolation Gravimetric Method | |
| US3509765A (en) | Inertial navigation system | |
| CN111102993A (en) | Initial alignment method for shaking base of rotary modulation type strapdown inertial navigation system | |
| Fu et al. | Information-reusing alignment technology for rotating inertial navigation system | |
| CN114111771A (en) | Dynamic attitude measurement method of double-shaft stable platform | |
| Fu et al. | Autonomous in-motion alignment for land vehicle strapdown inertial navigation system without the aid of external sensors | |
| WO2022006921A1 (en) | Data processing method for underwater strapdown gravity measurement | |
| CN111189442A (en) | Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF | |
| CN110285815A (en) | It is a kind of can in-orbit whole-process application micro-nano satellite multi-source information attitude determination method | |
| CN112902956A (en) | Course initial value acquisition method for handheld GNSS/MEMS-INS receiver, electronic equipment and storage medium | |
| CN108627152A (en) | A kind of air navigation aid of the miniature drone based on Fusion | |
| CN112325886A (en) | Spacecraft autonomous attitude determination system based on combination of gravity gradiometer and gyroscope | |
| Li et al. | Integrated calibration method for dithered RLG POS using a hybrid analytic/Kalman filter approach | |
| CN116105730A (en) | Angle measurement-only optical combination navigation method based on cooperative target satellite very short arc observation | |
| CN105300407B (en) | A kind of marine dynamic starting method for single axis modulation laser gyro inertial navigation system | |
| CN111141285B (en) | Aviation gravity measuring device | |
| RU2208559C1 (en) | Method of determination of inertial characteristics of spacecraft in the course of control by means of powered gyroscopes and jet engines | |
| CN116499492A (en) | DVL-assisted strapdown compass coarse alignment method under uniform-speed direct navigation | |
| CN111964671B (en) | Inertial astronomical integrated navigation system and method based on double-axis rotation modulation | |
| Hayal | Static calibration of the tactical grade inertial measurement units | |
| Radi et al. | GNSS Only Reduced Navigation System Performance Evaluation for High-Speed Smart Projectile Attitudes Estimation |
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 |