AU2020101268A4 - The initial alignment method for sway base - Google Patents
The initial alignment method for sway base Download PDFInfo
- Publication number
- AU2020101268A4 AU2020101268A4 AU2020101268A AU2020101268A AU2020101268A4 AU 2020101268 A4 AU2020101268 A4 AU 2020101268A4 AU 2020101268 A AU2020101268 A AU 2020101268A AU 2020101268 A AU2020101268 A AU 2020101268A AU 2020101268 A4 AU2020101268 A4 AU 2020101268A4
- Authority
- AU
- Australia
- Prior art keywords
- coordinate system
- velocity
- information
- alignment
- measurement
- 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.)
- Ceased
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manufacturing & Machinery (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The invention relates to an initial alignment method suitable for swinging and shaking base.
Under the condition of sway and shaking base, the angular rate information and specific force
information of triaxial gyroscope and triaxial accelerometer are collected synchronously in real
time; coarse alignment is carried out; the heading, pitch and roll information output by ins are
used to obtain the rotation relationship between body coordinate system and semi fixed
coordinate system; The acceleration information in the body coordinate system is projected
onto three axes in the semi fixed coordinate system and integrated; The signal is processed by
high pass digital filter; The extracted ship instantaneous line velocity information is used as the
velocity reference, and the difference between the extracted velocity and the velocity calculated
by the inertial navigation solution is used as the measurement of Kalman filter, and the
discretized state equation and measurement equation of Kalman filter are listed to complete the
alignment. The alignment time of the alignment method in the invention is shorter than the
alignment method measured by the position error; the alignment accuracy is higher; and the
environmental adaptability is stronger
-1/4
under the condition of sway and shaking base, the angular rate
information and specific force information of triaxial gyroscope and
triaxial accelerometer are collected synchronously in real time It
takes about 2 hours to complete the preheating preparation of SINS;
select the corresponding coarse alignment scheme, and complete the
coarse alignment;
after the coarse alignment, the transformation relationship between
the body coordinate system and the semi fixed coordinate system can
be obtained:
project the three axes acceleration information in the body coordinate
system to the semi fixed coordinate system, and integrate them to get
the speed information in the semi fixed coordinate system;
process the signal with high pass digital filter to get the high
frequen-cy signal relative to Schuler period, which is the
instantaneous line speed signal of ship;
take the extracted ship instantaneous line velocity information as the
velocity reference, take the difference with the velocity calculated by
the inertial navigation solution as the measurement of Kalman filter;
discrete state equation and measurement equation, and complete the
initialalignment.
Figure 1
Description
-1/4
under the condition of sway and shaking base, the angular rate information and specific force information of triaxial gyroscope and triaxial accelerometer are collected synchronously in real time It takes about 2 hours to complete the preheating preparation of SINS;
select the corresponding coarse alignment scheme, and complete the coarse alignment;
after the coarse alignment, the transformation relationship between the body coordinate system and the semi fixed coordinate system can be obtained:
project the three axes acceleration information in the body coordinate system to the semi fixed coordinate system, and integrate them to get the speed information in the semi fixed coordinate system;
process the signal with high pass digital filter to get the high frequen-cy signal relative to Schuler period, which is the instantaneous line speed signal of ship;
take the extracted ship instantaneous line velocity information as the velocity reference, take the difference with the velocity calculated by the inertial navigation solution as the measurement of Kalman filter;
discrete state equation and measurement equation, and complete the initialalignment.
Figure 1
PATENTS ACT 1990
The initial alignment method for sway base
The invention is described in the following statement:-
[0001] The invention relates to an initial alignment method of strapdown inertial navigation system for marine, in particular to an initial alignment method applicable to a swinging and shaking base.
[0002] Compared with spacecraft and aircraft, ship's sway and shake (also known as "instantaneous line motion") exist at the same time, which is a kind of peculiar bad motion state. Under the condition of instantaneous linear motion base, GPS and Doppler log cannot provide accurate instantaneous linear velocity information of carrier, which leads to inaccurate measurement (velocity information) in the process of Kalman filter alignment. If the instantaneous line motion is ignored, the initial alignment will be greatly affected.
[0003] For sway, the inertial navigation system must first produce the input of angular velocity, it will cause the change of attitude matrix C,' in the mathematical platform, but
C,' itself should change with the swing. Therefore, no matter how the load system swings,
the input angular velocity will only affect the C"' value, but will not affect the accuracy
of C". There are two parts in the line motion of a ship, one part is the line motion caused
by the ship's maneuvering, the other part is the instantaneous line motion caused by the wave, in the process of ship moving, because of the small amplitude of instantaneous line motion, the proportion of instantaneous line motion in the whole speed is very small, so this part can be ignored in the process of Kalman filter alignment. However, in the mooring state, due to the line motion caused by no maneuvering, the ship is always in the heave, sway and surge motion state caused by waves, that is in the instantaneous line motion state basically. At this time, this part of the motion cannot be ignored in the process of alignment. For heave and sway in different sea conditions, acceleration can be regarded as acting on the inertial navigation system, acceleration is introduced into the system, and the instantaneous velocity is obtained through integration of acceleration. For this kind of instantaneous line motion, there is not a very good measurement method in China at present (the velocity measurement error of DVL and GPS in mooring state is very large), which leads to a large measurement error of velocity in the process of Kalman filter alignment. For the Kalman filter method based on white noise error mode, it is equivalent to introducing a large amplitude of colored noise into the measurement, which makes the final estimation divergent. Therefore, how to extract the accurate instantaneous line motion information under the mooring condition becomes the key to solve the problem.
[0004] The invention provides an initial alignment method suitable for swinging and shaking base. Under the condition of sway and shaking base, the angular rate information and specific force information of triaxial gyroscope and triaxial accelerometer are collected synchronously in real time; coarse alignment is carried out; the heading, pitch and roll information output by ins are used to obtain the rotation relationship between body coordinate system and semi fixed coordinate system; The acceleration information in the body coordinate system is projected onto three axes in the semi fixed coordinate system and integrated; The signal is processed by high pass digital filter; The extracted ship instantaneous line velocity information is used as the velocity reference, and the difference between the extracted velocity and the velocity calculated by the inertial navigation solution is used as the measurement of Kalman filter, and the discretized state equation and measurement equation of Kalman filter are listed to complete the alignment. The alignment time of the alignment method in the invention is shorter than the alignment method measured by the position error; the alignment accuracy is higher; and the environmental adaptability is stronger.
[0005] The process to determine the characteristics of an initial alignment method for swinging and shaking base are as follows: Step 1: Under the condition of sway and shaking base, the angular rate information and specific force information of triaxial gyroscope and triaxial accelerometer are collected synchronously in real time;
Step 2: Do the coarse alignment; Step 3: After the coarse alignment, the heading, pitch and roll information output by the INS is used to get the transformation relationship between the body coordinate system and the semi fixed coordinate system; Step 4: Project the three axes acceleration information in the body coordinate system to the semi fixed coordinate system, and integrate them to get the speed information in the semi fixed coordinate system; Step 5: Process the signal with high pass digital filter to get the high frequency signal relative to Schuler period, which is the instantaneous line speed signal of ship; Step 6: Take the extracted ship instantaneous line velocity information as the velocity reference, take the difference with the velocity calculated by the inertial navigation solution as the measurement of Kalman filter, and the state equation and measurement equation of Kalman filter are obtained; Step 7: Discretize state equation and measurement equation, and complete the initial alignment.
[0006] Preferably, the initial alignment method for swinging and shaking base is preferably characterized in that: The coarse alignment is based on the inertial solidification hypothesis, the specific force information is projected into the base inertial coordinate system, the information of the direction change caused by the rotation of the earth relative to the acceleration of gravity is obtained. The positive and negative interference
accelerations are eliminated by integrating and the attitude matrix C, is solved.
[0007] Preferably, the initial alignment method for swing and shaking base is characterized in that the state equation and measurement equation of the Kalman filter are 1) state equation
X(t)= A- X(t)+ B-u(t)
The state vector is taken as
(t)= , <Po <p, V, V, V, ,, s, , The corresponding variables in the state vector are respectively east velocity error, north velocity error, vertical velocity error, pitch misalignment angle, roll misalignment angle, heading misalignment angle, x-axis accelerometer base, y-axis accelerometer base, z-axis accelerometer base, x axis gyro drift, y-axis gyro drift, z-axis gyro drift; where a, a, a. w, w w are the noise of accelerometer and gyroscope in the body coordinate system, which is normal distribution white noise with mean 0, andvarianceQ(t);
0 -£ £ Q 0 0 0 e q2 G 0 0 07a7 b2 42 b23 , 0 -£ 2 Q 0 0 0 C 0 0 0 a, b1 b2 b3 -f£ f 0 C GC C 0 0 0 U Cq 0 0 0 az 0 b4 2 b43 0 45 b6 0 0 0q Q Q 0 0 0Q Q Q 0 1 bl 0 0 b4 0 6 0 0 0 G C(2s 0 0 0 C C m, 0, _b 0 0 b b5 0 0 0 0 G 0Q+ 3000 Gq Gmo 0 0 0 0 0 0 0 0 0 0 0 0 V, 0 0 0 0 0 0 0 000 0 0 0 0 0 0 0 0 0 , 0 0 00 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 V 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 000 0 0 00 0 0 0 0 0 ' 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 _0]
Where R is the radius of the earth, t)i,, is the rotation rate of the earth, V is the north
speed, V is the east speed, V is the vertical velocity, 7 is heading angle;
b a= b12=2W, sin)y+ L tany b13 =-(2we cos)/ +L) R R R
V V V b21 =-2(w , sin y + O tan y ) b22 =-- b23 -= R R R
b3I = 2(w,, cos 7 + V) b32 = 2"j b3 =-2V w,, sin)y R R
b2 =0 b42= tany b45 = R,, sin)/ + V tan R R R
V 1 V, tany/ b4 6 -=-,COS)s+ b5 = -) b 4 =-osiny+ t
- tany bV V R R R R
fe is the east component of the specific force measured by the accelerometer projected in the geographical coordinate system;
f is the north component of the specific force measured by the accelerometer projected
in the geographical coordinate system;
f, is the vertical component of the specific force measured by the accelerometer
projected in the geographical coordinate system;
Co Ca C13 ]3 C2 C22 C2 3 is the transformation matrix from body coordinate system to C31 C32 C33]I
geographical coordinate system;
2) Measurement equation
The instantaneous linear velocity in the semi fixed coordinate system is taken as the velocity reference, and the difference between the projection of the velocity calculated by the INS and the velocity reference in the semi fixed coordinate system is taken as the measurement. The measurement equation of the system is as follows:
Z(t)= H(t)X(t)+v(t)
TI T T3 0 0 0 0 0 0 0 0 01 H(t)= T T T23 0 0 0 0 0 0 0 0 0 T1 T32 T33 0 0 0 0 0 0 0 0 0]
11I 12 13
T1 T T23 is the transformation matrix from geographic coordinate system _T1 T32 T33j to semi fixed coordinate system;
Among them, the measurement noise is v(t) and its variance is R(t).
[0008] Figure 1 is the schematic diagram;
[0009] Figure 2 is the flow chart;
[0010] Figure 3 is the schematic diagram of coarse alignment; and
[0011] Figure 4 shows the effect of initial alignment, in which the dotted line represents the method proposed in the invention and the solid line represents the traditional Kalman filtering method.
[0012] The aim of the invention is to provide an initial alignment method suitable for swinging and shaking base with short alignment time, strong environmental adaptability and high alignment accuracy.
[0013] The aim of the invention is realized as follows:
Step 1: Under the condition of swinging and shaking base, the angular rate information and specific force information of triaxial gyroscope and triaxial accelerometer are collected synchronously in real time;
Step 2: Do the coarse alignment;
Step 3: After the coarse alignment, the heading, pitch and roll information output by the INS is used to get the transformation relationship between the body coordinate system and the semi fixed coordinate system;
Step 4: Project the three axes acceleration information in the body coordinate system to the semi fixed coordinate system, and integrate them to get the speed information in the semi fixed coordinate system;
Step 5: Process the signal with high pass digital filter to get the high frequency signal relative to Schuler period, which is the instantaneous line speed signal of ship;
Step 6: Take the extracted ship instantaneous line velocity information as the velocity reference, take the difference with the velocity calculated by the inertial navigation solution as the measurement of Kalman filter, and the state equation and measurement equation of Kalman filter are obtained;
Step 7: Discretize state equation and measurement equation, and complete the initial alignment.
[0014] Under the condition of swaying and shaking disturbance base, the Doppler log and GPS cannot provide accurate velocity information, which results in the failure of convergence of Kalman initial alignment measured by velocity error. In order to solve this problem, the invention is based on coarse alignment, and uses the inertial measurement method, by designing the corresponding digital filter, the instantaneous line motion information is extracted, take the speed as measurement reference to complete the initial alignment.
[0015] The advantages of the invention are mainly embodied in:
[0016] 1. Compared with the Kalman filter method based on the measurement of position error, the method proposed in the invention has the advantages that the coupling degree between speed error and attitude error is closer than that between position error and attitude error, so the alignment time is shorter than that of the alignment method based on the measurement of position error.
[0017] 2. Because the velocity errors measured by GPS and DVL are too large to be used as reference under the swinging and shaking base, the method in the invention is more adaptable to the environment than the traditional the velocity errors measurement Kalman filter method.
[0018] 3. Because the information used as the speed reference is more accurate, compared with the traditional velocity error measurement Kalman filter method, the alignment accuracy of this method is higher.
[0019] 4. Because the information used as the speed reference is more accurate, compared with the traditional velocity error measurement Kalman filter method, the convergence speed of the filter is faster and the alignment time is effectively shortened.
Specific implementation mode
[0020] The invention is described in more detail as follows:
[0021] Step 1: Under the condition of sway and shaking base, the angular rate information and specific force information of triaxial gyroscope and triaxial accelerometer are collected synchronously in real time. It takes about 2 hours to complete the preheating preparation of SINS;
[0022] Step 2: according to how to eliminate the interference acceleration, select the corresponding coarse alignment scheme, and complete the coarse alignment;
[0023] How to effectively remove the interference acceleration caused by shaking is the key to coarse alignment, because the rolling motion of the carrier will also lead to the variation of the specific force information measured by the accelerometer, which reflects the variation of the heading and attitude of the carrier due to the normal swinging motion. Therefore, the change of acceleration caused by swing cannot be filtered out, and the filtering cannot be carried out in the body coordinate system. In order to overcome the defect of analytical coarse alignment, the coarse alignment base on inertial coordinate system is designed, which keeps the change of acceleration caused by swing and only filters out the interference acceleration of instantaneous linear motion.
[0024] This method applies the inertial solidification hypothesis. The specific force information is projected into inertial coordinate system (at this time, the output of the accelerometer does not contain the acceleration change caused by the swing motion), and the direction change information of the gravity acceleration relative to the inertial space caused by the rotation of the earth is obtained. By integrating, the positive and negative interference accelerations are eliminated, the attitude matrix can be obtained.
[0025] Define a new coordinate system: body inertial coordinate system 'O, It is obtained by fix body coordinate system b at to , where to is the start time of coarse alignment. The strapdown matrix can be expressed as:
C1" = C"CCLC C 1 )11
Where , CCe and C, are the transition matrix between i coordinate system, n coordinate system and e coordinate system at time t, they can be calculated by the postion
information and coruse alignment time t. C" is the transition matrix between b coordinate
system and '1- coordinate system, Using the angular rate information in b coordinate
system relative to 'o system output by gyroscope, the matrix can be obtained by attitude
updating algorithm of strapdown inertial navigation. c- is the transition matrix between
'1 coordinate system and i coordinate system, and it is a constant matrix. It can be obtained from the conversion relationship between the acceleration of gravity and the output of the
accelerometer. The calculation of the matrix is the key to get the attitude matrix c;. In this method, the calculation of strapdown attitude matrix which changes with time is transformed into calculation the inertial coordinate system which does not change with time.
1) Calculate C" and C7
C"e is the transition matrix between e coordinate system and n In the equation (1),
coordinate system, and it can be calculate by the latitude Y and longitude A where the carrier location.
1 0 0 7 sinA cosA 0] -sinA cosA 07 C"= 0 sin cos Cos A sin A 0 = -sincosA -sine sinl cos e (2) 0 -cos singj_ 0 0 1 _cos ocosA cos osinA singo]
C," is the transition matrix between the e coordinate system and i
coordinatesystem, it can be calculated by time interval At = t - to
cosjeAt sinOieAt 0] C ! sinw,At coswiAt 0 (3) 0 0 l3
2) Calculate C"°
C"° is the transition matrix between io coordinate system and b coordinate system. Initial time C'1°(to)= I, C,"' can be calculated by the angular rate information in b
coordinate system relative to 'ho coordinate system output by gyroscope, and can be updated by four element updating solution.
3) Calculate C
C, is the transition matix between i coordinate system and ibo coordinate system. Due to C," , 'C, can be calculated with known information. The C," can be
calculated if C is obtained, then, coarse alignment is complete. Therefore, the task of
coarse alignment is changed from solving Gb to solving constant matrix
On a swinging base, the projection of the accelerometer output in the 'ho coordinate system can be expressed as follows:
f .!° = C. f (4)
Where 1" is the output specific force of the accelerometer. Under the condition of instantaneous linear motion, the accelerometer can sense two components: The interference
acceleration and gravity acceleration of instantaneous linear motion, LetVb be the bias of acceleration, then the specific force output of accelerometer is:
j- gb+a+Vb (5) b Where aD is the acceleration of interference line motion caused by ship surge, heave, sway and high frequency interference. Substituting equation (5) into equation (4):
f'I° = C = +C° (aD+Vb) (6)
The apparent velocity in the base inertial system is obtained by integrating the two
sides of equation (4) in the interval of [t t ]:
tk tk tA tk tk Yib° = Cib|jdt= -Cgdt+C(ab+Vb)dt=C>" -g'dt+j C(ab+Vb)dt (7) to to to to to
Make V'=J- g'dt , SV"°o=f Cf° (ab+V b)dt , because interference ab is to to
approximately periodic:
SV'i°o=JCf°(ab+Vb)dt 0 (8) to
Substituting formula (8) into formula (7), there are:
V'o = Cbo Vi (9)
And:
-g cos q cos[ I+o>,(t -to)]] g'= -gcosqasin[A+o>,(t-to)] (10) -gsinqp
Where oe is the rotation angular rate of the earth, g is the acceleration of gravity.
LetAtk-t-- to,(11) can be obtained:
gCose([sin(2+woiAt)-sin2]] Nie iAtk)](1 V'c(t) g Cos eo 1cos A - cos(+ il g sin eoAtk
From equation (9), in time tk , tk2 (to < I <tk2)
V'b° (tk)= C :° V'(tl) (12)
Y'bo(tk2)=C|"Vi(t 2) (13)
It can be proved that:
YJ'i° (tkl)x bo(tk2 )=(C|"°V'(tk1)]X [C"°V'(tk )1 2 (14) = boV'(tkl)XV'(tk 2 )
V' Represents the velocity in the geocentric inertial coordinate system, 1'"V is the apparent velocity in the inertial body coordinate system, and there are =C/°V'. In
order to solve all nine elements of matrix , we need to construct new vectors to increase the number of equations.
In ' =C°V take different time t=tkl and t=tk2 in alignment process, for
example, the middle time and the last time, then f'b°(t ,)=C/°V'(tkl)
V'b (tk2)=C"V'(t 2 ), The constant matrix C ° can be obtained by using the matrix construction algorithm.
[jibo(t )]
[V'(tkl)]T
S= V'(tk2 )] ibo(tk 2)] (15)
[Vi(tkl ) XV(tk 2 ) ] _[1i4o(tkl ) XV"' 0o(t2
V'(tkl k2) and V'"° (tkI )Xb "0t2 )are the cross product vector in time tkl and
tk2 V (tkI) and V'(tk 2 ) "Oi(tkl) and V'" (tk2) are required not parallel, otherwise,
the cross product is meaningless. So tkl and tk2 can't choose too close to avoid parallel two vectors. Generally, the middle time and the last time in the whole alignment process
are selected. When C; is obtained, Substituting it to formula (1), C can be get. After the coarse alignment task, the attitude error can be reduced to less than 2 degrees.
[0026] Step 3: after the coarse alignment, the heading, pitch and roll information output by the INS is used to get the transformation relationship between the body coordinate system and the semi fixed coordinate system;
[0027] The navigation information obtained by the inertial navigation system is based on the body coordinate system, the description of instantaneous line motion is completed in semi fixed coordinate system. Therefore, it needs to be projected from the body coordinate system to the semi fixed coordinate system, let the heading, pitch and roll obtained after
the coarse alignment are y,a,, respectively. Rotational relationship between semi fixed coordinate system OjY1Z, and body coordinate system OX,,Z, can be expressed as
follows:
xd Xb
Yd, Cb b ( 16) Zd _ h
Where Cj is direction cosine matrix transformed from body coordinate system to semi fixed coordinate system:
cosy cos§8 sinycosa+cosysin3sina sinysina-cosysin/3cosal C'd= -sinycosp8 cosycosa+sinysinp8sina cosysina-sinysinp8cosa (17) sin§8 -cos/3sina cos/3cosa j Step 4: Project the three axes acceleration information in the body coordinate system to the semi fixed coordinate system, and integrate them to get the speed information in the semi fixed coordinate system;
[0028] Step 5: Process the signal with high pass digital filter to get the high frequency signal relative to Schuler period, which is the instantaneous line speed signal of ship;
[0029] The velocity information obtained from step 4 includes the instantaneous line motion information and the Shura period oscillation component (the oscillation period is 84.4min). Compared with the ship's moving, the ship's instantaneous line motion belongs to the high frequency motion, with a short period of motion, generally 1.5s-10s. It is assumed that the period of ship instantaneous line motion is 1.5s and the frequency is 0.67Hz. 1/5 (0.13Hz) of the instantaneous line motion frequency of the ship is selected as the cut-off frequency, and the corresponding digital filter is designed. The velocity information in semi fixed coordinate system is filtered.
[0030] Step 6: Take the extracted ship instantaneous line velocity information as the velocity reference, take the difference with the velocity calculated by the inertial navigation solution as the measurement of Kalman filter, and the state equation and measurement equation of Kalman filter are obtained; 1) state equation
k(t)= A - X(t)+ B- u(t)
The state vector is taken as
X(t)=[SV ' SV ' V'<, P<, V, V, V, , e, S4 , The corresponding
variables in the state vector are respectively east velocity error, north velocity error, vertical velocity error, pitch misalignment angle, roll misalignment angle, heading misalignment angle, x-axis accelerometer base, y-axis accelerometer base, z-axis accelerometer base, x axis gyro drift, y-axis gyro drift, z-axis gyro drift; where a, , az w co, co, are the
noise of accelerometer and gyroscope in the body coordinate system, which is normal distribution white noise with mean 0 and variance Q(t); b b 2, f 0 -f C C,G 0 0 0 C,, C .C C 0 0 0 a, 2 3 32 I3I 0 0 0 C" 32 3" z
0 b2 b 0 b, b. 0 0 020134 0 0 0 0 b, 0 0 b, 0 b, 0 0 0 C, C Q 0, 0 0 0 G GQ. b(, 0 0 b(, b, 0 0 0 0 C, C32 C,3 1 0 0 0 C, C2 3
0 0 0 0 0 0 0 0 0 0 0 V 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0V, 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0V 00000 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 00000 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ] 0 0 0 0 00 J O]
Where R is the radius of the earth, a, is the rotation rate of the earth, V is the north
speed, V, is the east speed, V, is the vertical velocity, 7 is heading angle;
Vtanyb V V b R="2 2mie siny+ etany b13=-(2mi, cosy+ e) R R R
b2I =-2(w, sin /+ e tany) b22 = - b23 R R R
V~ 2V b3 = 2(wi cos7 + b2 = n b3 e=s-2Vew,, sin)y R R
1 tany V tany b42 = R 42 R Rsiy R lOi i
V 1V tan)y b4 6 = -(we cos7 + ) b = b4 = -(wi sin)y + aR) R R R
V tan y V= V= R 61 R 64RY6 R
fe is the east component of the specific force measured by the accelerometer projected
in the geographical coordinate system; f is the north component of the specific force measured by the accelerometer projected in the geographical coordinate system; f, is the vertical component of the specific force measured by the accelerometer projected in the geographical coordinate system;
Cu Ca C13 ]3 C2 C22 C2 3 is the transformation matrix from body coordinate system to
C2e C32 C33 I geographical coordinate system;
2) Measurement equation
The instantaneous linear velocity in the semi fixed coordinate system is taken as the velocity reference, and the difference between the projection of the velocity calculated by the INS and the velocity reference in the semi fixed coordinate system is taken as the measurement. The measurement equation of the system is as follows:
Z(t)= H(t)X(t)+v(t)
TI T2 T 0 0 0 0 0 0 0 0 07 H(t)= T T T23 0 0 0 0 0 0 0 0 0 T1 T32 T33 0 0 0 0 0 0 0 0 0]
T T12 Ts31 T2 T T23 is the transformation matrix from geographic coordinate system _T1 T3 2 T33I to semi fixed coordinate system;
Among them, the measurement noise is v(t) and its variance is R(t).
[0031] Step 7: Discretize state equation and measurement equation, and complete the initial alignment.
[0032] Discretize state equation and measurement equation,
X(k+1)=<D(kk-1)X(k)+F,,,(t)W(k) (18) Z(k)=H- X(k)+V(k)
Where,
'J(k+1,k)= I+ A(t)At +O(At2
) Fk+,k =J (k+,k)G(r)dt
W(k)=W(tk) V(k)=Y(tk)
Apply the standard Kalman filter equation:
k/k-I = 'k/k-I kk-I
p k/k-I = cp )k/k-I (T_ k_ k -I +
K k =P HT(H P 1H+R)-I Kk k Thk kk~kIH+R (19) Xk=k kI+K(Zk -kk-1)
P =(I-KH I(I-KHk)T +KR*K
Claims (3)
1. The process to determine the characteristics of an initial alignment method for swinging and shaking base are as follows:
Step 1: Under the condition of sway and shaking base, the angular rate information and specific force information of triaxial gyroscope and triaxial accelerometer are collected synchronously in real time;
Step 2: Do the coarse alignment;
Step 3: After the coarse alignment, the heading, pitch and roll information output by the INS is used to get the transformation relationship between the body coordinate system and the semi fixed coordinate system;
Step 4: Project the three axes acceleration information in the body coordinate system to the semi fixed coordinate system, and integrate them to get the speed information in the semi fixed coordinate system;
Step 5: Process the signal with high pass digital filter to get the high frequency signal relative to Schuler period, which is the instantaneous line speed signal of ship;
Step 6: Take the extracted ship instantaneous line velocity information as the velocity reference, take the difference with the velocity calculated by the inertial navigation solution as the measurement of Kalman filter, and the state equation and measurement equation of Kalman filter are obtained;
Step 7: Discretize state equation and measurement equation, and complete the initial alignment.
2. The process of claim 1, wherein the initial alignment method for swinging and shaking base is characterized in that: The coarse alignment is based on the inertial solidification hypothesis, the specific force information is projected into the base inertial coordinate system, the information of the direction change caused by the rotation of the earth relative to the acceleration of gravity is obtained. The positive and negative interference accelerations
are eliminated by integrating and the attitude matrix C'" is solved.
3. The process of claim 1 or 2, wherein the initial alignment method for swinging and shaking base is characterized in that the state equation and measurement equation of the Kalman filter are
1) state equation
i'(t)= A- X(t)+ B. u(t)
The state vector is taken asX(t) =[b, bV 5V, V,. V, Ve,6
, The corresponding variables in the state vector are respectively east velocity error, north velocity error, vertical velocity error, pitch misalignment angle, roll misalignment angle, heading misalignment angle, x-axis accelerometer base, y-axis accelerometer base, z-axis accelerometer base, x-axis gyro drift, y-axis gyro drift, z-axis gyro drift; where ax a, a. w, w, w, are the noise of accelerometer and gyroscope in the body coordinate system, which is normal distribution white noise with mean 0, and variance Q(t)
k b k 0 -£ f£ qI q2Q 0 0 0 e QI Q G 0 0 0 ax b2 b2b £3X 0 -£ C22C23 0 0 0 C2 CC (C3 0 0 0 a, bb~ b0q~c~-£OhI b bb 42 0 0q~ CqC2C 0C 0 0 0 az e 0 61 b2 b 0 00 624 65, q QQe 0 0 0 qQ 0 I q q , b1 0 0 b 0 b6 0 0 0 CqI C q4G,1 0 0 0 CI C qCt
0 0 0 00 0 0 0 0 000qq 0 , 0000000
* 000000 0000000V, 000000 0 0 0 0 0 0 0 0 0 0 0 0 0 V 0 0 0 0 0 0 0 . 0 0 0 0 0 0 0 0 0 0 0 0 s 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ] 0 0 0 0 0 0 0 0 0 0 0 0 0 _0)
'(*z I Where R is the radius of the earth, w,, is the rotation rate of the earth, V, is the north speed,
V, is the east speed, V, is the vertical velocity, y is heading angle;
b = V tany b 2= 2w, siny+ tany b3 = -(2w,, cosy + )
R R R
V V V bn =-2(w,siny+ 'tany) b2= b2 = R R R "
bI = 2(w,, cos)y +V ) b2 - 2 b3 =-2V w,, sin)y R R
1tan y bVwsn+ /tan y b42 = R b42 = Ra) b45 = o), sin)y+ R ta) b2R =
b4 6 =-(0),,COS/V b5 1 b5 4 =-(o),,sin)/ tan)/ R R R
Vj tan y 64 V b56 = " b = b6 = Cos)y+ 5b6 =
" R R R R
f is the east component of the specific force measured by the accelerometer projected in the geographical coordinate system;
f, is the north component of the specific force measured by the accelerometer projected in
the geographical coordinate system;
f, is the vertical component of the specific force measured by the accelerometer projected in the geographical coordinate system;
C C12 C13I C21 C22 C23 is the transformation matrix from body coordinate system to geographical CL1 C32 C3I coordinate system;
2) Measurement equation
The instantaneous linear velocity in the semi fixed coordinate system is taken as the velocity reference, and the difference between the projection of the velocity calculated by the INS and the velocity reference in the semi fixed coordinate system is taken as the measurement. The measurement equation of the system is as follows:
Z(t)= H(t)X(t)+v(t)
TI T2 T 0 0 0 0 0 0 0 0 07 H(t)= T1 T T3 0 0 0 0 0 0 0 0 0 HT1 T2 T3 0 0 0 0 0 0 0 0 0]
T T2 T3 I
T T T is the transformation matrix from geographic coordinate system to T1 T2 T3 I
semi fixed coordinate system;
Among them, the measurement noise is v(t) and its variance is R(t).
-1/4-
Figure 1
-2/4- 2020101268
Gyro output
Coarse INS Accelerometer alignment update Kalman output filter Velocity information in High pass semi fixed digital filter coordinate system
Figure 2
-3/4- 2020101268
bi Cbib0
Cbi b0
Ciib 0 Cbn Cni
Cie
Cni Cen
Figure 3
yaw/° roll/° pitch/° -4/4-
time/min
Figure 4
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| AU2020101268A AU2020101268A4 (en) | 2020-07-06 | 2020-07-06 | The initial alignment method for sway base |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| AU2020101268A AU2020101268A4 (en) | 2020-07-06 | 2020-07-06 | The initial alignment method for sway base |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| AU2020101268A4 true AU2020101268A4 (en) | 2020-08-13 |
Family
ID=71949481
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| AU2020101268A Ceased AU2020101268A4 (en) | 2020-07-06 | 2020-07-06 | The initial alignment method for sway base |
Country Status (1)
| Country | Link |
|---|---|
| AU (1) | AU2020101268A4 (en) |
Cited By (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112033438A (en) * | 2020-08-18 | 2020-12-04 | 湖北航天技术研究院总体设计所 | Shaking base self-alignment method based on speed fitting |
| CN112033439A (en) * | 2020-08-20 | 2020-12-04 | 哈尔滨工业大学 | Gravity acceleration vector weftless construction method under swinging base geosystem |
| CN112729332A (en) * | 2020-11-17 | 2021-04-30 | 中国船舶重工集团公司第七0七研究所 | Alignment method based on rotation modulation |
| CN112747772A (en) * | 2020-12-28 | 2021-05-04 | 厦门华源嘉航科技有限公司 | Request-based inertial odometer moving base coarse alignment method |
| CN112857398A (en) * | 2021-01-12 | 2021-05-28 | 中国人民解放军海军工程大学 | Rapid initial alignment method and device for ships in mooring state |
| CN113236363A (en) * | 2021-04-23 | 2021-08-10 | 陕西陕煤黄陵矿业有限公司 | Mining equipment navigation positioning method, system, equipment and readable storage medium |
| CN113405563A (en) * | 2021-05-25 | 2021-09-17 | 北京机械设备研究所 | Inertial measurement unit alignment method |
| CN113503895A (en) * | 2021-06-10 | 2021-10-15 | 北京自动化控制设备研究所 | Kalman filtering-based three-autonomous inertial measurement unit accelerometer size estimation method |
| CN113503893A (en) * | 2021-06-02 | 2021-10-15 | 北京自动化控制设备研究所 | Initial alignment algorithm of moving base inertial navigation system |
| CN113884091A (en) * | 2021-08-24 | 2022-01-04 | 江苏无线电厂有限公司 | Method for transferring attitude angle of photoelectric stabilized platform and storage medium |
| CN113959462A (en) * | 2021-10-21 | 2022-01-21 | 北京机电工程研究所 | Quaternion-based inertial navigation system self-alignment method |
| CN114111771A (en) * | 2021-11-25 | 2022-03-01 | 九江中船仪表有限责任公司（四四一厂） | Dynamic attitude measurement method of double-shaft stable platform |
| CN115031785A (en) * | 2022-06-21 | 2022-09-09 | 浙江大学 | Soft soil surveying method based on multi-sensor fusion technology |
| CN115200574A (en) * | 2022-07-03 | 2022-10-18 | 中国人民解放军国防科技大学 | Polar region transverse combination navigation method under earth ellipsoid model |
| CN115855038A (en) * | 2022-11-22 | 2023-03-28 | 哈尔滨工程大学 | Short-time high-precision attitude keeping method |
| CN115856946A (en) * | 2023-02-21 | 2023-03-28 | 和普威视光电股份有限公司 | Aircraft alignment channel detection method, device, terminal and storage medium |
| CN116026370A (en) * | 2023-03-30 | 2023-04-28 | 中国船舶集团有限公司第七〇七研究所 | Matrix equivalent conversion-based fiber-optic gyroscope error calibration method and system |
-
2020
- 2020-07-06 AU AU2020101268A patent/AU2020101268A4/en not_active Ceased
Cited By (27)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112033438A (en) * | 2020-08-18 | 2020-12-04 | 湖北航天技术研究院总体设计所 | Shaking base self-alignment method based on speed fitting |
| CN112033438B (en) * | 2020-08-18 | 2022-09-02 | 湖北航天技术研究院总体设计所 | Shaking base self-alignment method based on speed fitting |
| CN112033439A (en) * | 2020-08-20 | 2020-12-04 | 哈尔滨工业大学 | Gravity acceleration vector weftless construction method under swinging base geosystem |
| CN112729332A (en) * | 2020-11-17 | 2021-04-30 | 中国船舶重工集团公司第七0七研究所 | Alignment method based on rotation modulation |
| CN112729332B (en) * | 2020-11-17 | 2022-10-28 | 中国船舶重工集团公司第七0七研究所 | Alignment method based on rotation modulation |
| CN112747772B (en) * | 2020-12-28 | 2022-07-19 | 厦门华源嘉航科技有限公司 | Request-based inertial odometer moving base coarse alignment method |
| CN112747772A (en) * | 2020-12-28 | 2021-05-04 | 厦门华源嘉航科技有限公司 | Request-based inertial odometer moving base coarse alignment method |
| CN112857398A (en) * | 2021-01-12 | 2021-05-28 | 中国人民解放军海军工程大学 | Rapid initial alignment method and device for ships in mooring state |
| CN112857398B (en) * | 2021-01-12 | 2023-04-28 | 中国人民解放军海军工程大学 | Rapid initial alignment method and device for ship under mooring state |
| CN113236363A (en) * | 2021-04-23 | 2021-08-10 | 陕西陕煤黄陵矿业有限公司 | Mining equipment navigation positioning method, system, equipment and readable storage medium |
| CN113405563A (en) * | 2021-05-25 | 2021-09-17 | 北京机械设备研究所 | Inertial measurement unit alignment method |
| CN113405563B (en) * | 2021-05-25 | 2023-09-05 | 北京机械设备研究所 | Inertial measurement unit alignment method |
| CN113503893A (en) * | 2021-06-02 | 2021-10-15 | 北京自动化控制设备研究所 | Initial alignment algorithm of moving base inertial navigation system |
| CN113503895B (en) * | 2021-06-10 | 2023-08-15 | 北京自动化控制设备研究所 | Three-self inertial measurement unit accelerometer size estimation method based on Kalman filtering |
| CN113503895A (en) * | 2021-06-10 | 2021-10-15 | 北京自动化控制设备研究所 | Kalman filtering-based three-autonomous inertial measurement unit accelerometer size estimation method |
| CN113884091A (en) * | 2021-08-24 | 2022-01-04 | 江苏无线电厂有限公司 | Method for transferring attitude angle of photoelectric stabilized platform and storage medium |
| CN113884091B (en) * | 2021-08-24 | 2024-01-09 | 江苏无线电厂有限公司 | Method for transmitting attitude angle of photoelectric stabilized platform and storage medium |
| CN113959462A (en) * | 2021-10-21 | 2022-01-21 | 北京机电工程研究所 | Quaternion-based inertial navigation system self-alignment method |
| CN113959462B (en) * | 2021-10-21 | 2023-09-12 | 北京机电工程研究所 | Quaternion-based inertial navigation system self-alignment method |
| CN114111771A (en) * | 2021-11-25 | 2022-03-01 | 九江中船仪表有限责任公司（四四一厂） | Dynamic attitude measurement method of double-shaft stable platform |
| CN115031785A (en) * | 2022-06-21 | 2022-09-09 | 浙江大学 | Soft soil surveying method based on multi-sensor fusion technology |
| CN115200574A (en) * | 2022-07-03 | 2022-10-18 | 中国人民解放军国防科技大学 | Polar region transverse combination navigation method under earth ellipsoid model |
| CN115200574B (en) * | 2022-07-03 | 2024-04-19 | 中国人民解放军国防科技大学 | Polar region transverse combined navigation method under earth ellipsoid model |
| CN115855038B (en) * | 2022-11-22 | 2024-01-09 | 哈尔滨工程大学 | Short-time high-precision posture maintaining method |
| CN115855038A (en) * | 2022-11-22 | 2023-03-28 | 哈尔滨工程大学 | Short-time high-precision attitude keeping method |
| CN115856946A (en) * | 2023-02-21 | 2023-03-28 | 和普威视光电股份有限公司 | Aircraft alignment channel detection method, device, terminal and storage medium |
| CN116026370A (en) * | 2023-03-30 | 2023-04-28 | 中国船舶集团有限公司第七〇七研究所 | Matrix equivalent conversion-based fiber-optic gyroscope error calibration method and system |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| AU2020101268A4 (en) | The initial alignment method for sway base | |
| CN107656301B (en) | Vehicle-mounted positioning method based on multi-source information fusion | |
| CN106767787A (en) | A kind of close coupling GNSS/INS combined navigation devices | |
| CN107015259B (en) | Method for calculating pseudorange/pseudorange rate by using Doppler velocimeter | |
| CN102721417B (en) | Method for restraining coarse alignment error of solidification inertial system of strapdown inertial navigation system | |
| CN112505737B (en) | GNSS/INS integrated navigation method | |
| CN108594283B (en) | Free installation method of GNSS/MEMS inertial integrated navigation system | |
| CN104698485B (en) | Integrated navigation system and air navigation aid based on BD, GPS and MEMS | |
| CN113203418B (en) | GNSSINS visual fusion positioning method and system based on sequential Kalman filtering | |
| CN102608642A (en) | Beidou/inertial combined navigation system | |
| CN109506660B (en) | Attitude optimization resolving method for bionic navigation | |
| CN109059911A (en) | A kind of GNSS, INS and barometrical data fusion method | |
| Hide et al. | GPS and low cost INS integration for positioning in the urban environment | |
| CN111722295B (en) | Underwater strapdown gravity measurement data processing method | |
| CN104931994A (en) | Software receiver-based distributed deep integrated navigation method and system | |
| CN110221331B (en) | Inertia/satellite combined navigation dynamic filtering method based on state transformation | |
| CN115388884A (en) | Joint initialization method for intelligent body pose estimator | |
| Nagui et al. | Improved GPS/IMU loosely coupled integration scheme using two kalman filter-based cascaded stages | |
| Xiang et al. | A SINS/GNSS/2D-LDV integrated navigation scheme for unmanned ground vehicles | |
| CN104634348B (en) | Attitude angle computational methods in integrated navigation | |
| CN115542363B (en) | Attitude measurement method suitable for vertical downward-looking aviation pod | |
| CN115523920B (en) | Seamless positioning method based on visual inertial GNSS tight coupling | |
| CN104344835A (en) | Serial inertial navigation moving base alignment method based on switching type self-adaptive control compass | |
| CN112083425A (en) | SINS/LBL tight combination navigation method introducing radial velocity | |
| Luo et al. | An integrated navigation and localization system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| FGI | Letters patent sealed or granted (innovation patent) | ||
| MK22 | Patent ceased section 143a(d), or expired - non payment of renewal fee or expiry |