CN103575300B - Rotating modulation-based rocking base inertial navigation system coarse alignment method - Google Patents

Abstract

The invention provides a rotating modulation-based rocking base inertial navigation system coarse alignment method which can be used for improving the accuracy of coarse alignment in case of a rocking base. The method comprises the following steps: 1, periodically rotating double shafts of a rotary inertial navigation system according to a preset rotation scheme in a coarse alignment process; 2, solving an attitude matrix estimated value of IMU (Inertial Measurement Unit) in each sampling period of the IMU by using an analytic method; 3, performing attitude matrix updating in each sampling period according to angular velocity output by a gyroscope to solve a direction cosine matrix from a local solidification IMU coordinate system to an IMU coordinate system at the current moment; 4, solving an estimated value of the constant direction cosine matrix from the local solidification IMU coordinate system to the IMU coordinate system in the sampling period; 5, solving a final estimated value of the direction cosine matrix from the local solidification IMU coordinate system to the IMU coordinate system; 6, obtaining the attitude matrix of the IMU at the end moment of the coarse alignment.

Description

A kind of swaying base inertial navigation system coarse alignment method based on rotation modulation

Technical field

The invention belongs to Methods of Strapdown Inertial Navigation System technical field, it is related to a kind of swaying base inertial navigation system based on rotation modulation System coarse alignment method.

Background technology

The initial alignment process of Methods of Strapdown Inertial Navigation System is divided into two stages：Coarse alignment and fine alignment.Coarse alignment main Task is to estimate the approximation of attitude matrix, provides a basic reference mathematical platform for SINS；In fine alignment rank Section estimates the error of attitude matrix further by methods such as state filterings, thus obtaining more accurate mathematical platform, here On the basis of carry out navigation calculation.The precision of coarse alignment can shadow to the convergence rate of fine alignment and precision, and then affect navigational solution The precision calculated, therefore improves coarse alignment precision significant to inertial navigation system performance.

Traditional analytic expression coarse alignment calculates the attitude matrix initial value of carrier according to the output of gyroscope and accelerometer, but When carrier such as is shaken at the random disturbances, inertia device output signal-to-noise ratio reduces, and alignment precision declines.Conventional research and utilization Coarse alignment method based on solidification inertial coodinate system suppresses carrier to vibrate interference signal by integral operation.But accelerometer exports In also contain zero offset error in addition to agitation error, with integration accumulation can still result in coarse alignment precision reduce.And agitation error Compensation again relies on above-mentioned integral process, so the solidification inertial system coarse alignment method of inertial navigation has certain limitation.

Obtain extensively should in inertial navigation field based on the Error Compensation Technology that Inertial Measurement Unit (IMU) rotates in recent years With.Rotary inertia guiding systems have the ring stand similar to platform inertial navigation, rotate the suppression system accumulation of error by IMU；Simultaneously Rotating mechanism also provides controlled angular movement characteristic to IMU, can assist the rotary inertial alignment observability of improvement. We can be by means of solidification inertial system it is assumed that and introduce coarse alignment link by rotation modulation technology, by the inertia of two passages Device zero is modulated into sinusoidal signal, then zero after endless unit impulse response (IIR) low pass filter filters modulation partially Partially, thus suppressing coarse alignment error.But the method can only suppress the zero of passage portion partially, and introduce wave filter will increase slightly right Quasi- operand, thus increasing the complexity of navigation system and affecting the real-time being aligned.

Content of the invention

For the coarse alignment of the carrier of oscillation on small scale, the present invention proposes a kind of swaying base inertial navigation system based on rotation modulation System coarse alignment method, improves the precision of coarse alignment under swaying base, and the method realizes simplicity, and operand is low, and suitable engineering should With.

The swaying base inertial navigation system coarse alignment method based on rotation modulation for this kind, comprises the following steps：

The first step：The twin shaft making rotary inertia guiding systems during coarse alignment is periodically revolved by default rotation approach Turn；

Second step：The attitude matrix estimated value of IMU is all obtained in each sampling period of IMU using analytic method；

3rd step：In each sampling period, attitude matrix renewal is carried out according to the angular velocity that gyro exports and obtain current time It is tied to the direction cosine matrix of IMU coordinate system from local solidification IMU coordinate；

4th step：It is tied to from local solidification IMU coordinate according to current time IMU attitude matrix estimated value and current time The direction cosine matrix of IMU coordinate system obtains the constant value side that this sampling period is tied to geographic coordinate system from local solidification IMU coordinate Estimated value to cosine matrix；

5th step：The constant value being tied to geographic coordinate system from local solidification IMU coordinate that each moment tries to achieve to coarse alignment process Direction cosine matrix is averaged, obtain from local solidification IMU coordinate be tied to geographic coordinate system direction cosine matrix final Estimated value；

6th step：The constant value direction cosines being tied to geographic coordinate system from local solidification IMU coordinate tried to achieve according to the 5th step Matrix Multiplication is tied to the direction of local solidification IMU coordinate system more than by what posture renewal obtained from IMU coordinate with coarse alignment finish time String matrix, that is, obtain the attitude matrix of coarse alignment finish time IMU.

In the first step, default rotation approach includes following scheme：

A. inner axle, the unidirectional continuous rotation of outer annulate shaft；

B. inner axle, outer annulate shaft continuously rotate, and the change that often rotates a circle turns to；

C. inner axle, outer annulate shaft is unidirectional is alternately rotated, each axle rotates a circle, and stops starting simultaneously at another axle of rotation, such as This moves in circles；

D. inner axle, outer annulate shaft break-in are alternately rotated, and first axle stops after rotating a circle, and then rotates one by the second axle In week, then reversely rotated one week by first axle again, then reversely rotated one week by the second axle again, so move in circles；

E. inner axle, outer annulate shaft break-in are alternately rotated, and first axle reversely rotates one week after rotating a circle again, then stops, Then reversely rotate again after being rotated a circle by the second axle one week, so move in circles；

Wherein scheme a, c only can use in the case that the rotation platform of Rotating Inertial Navigation System contains conducting slip ring, and And can not adopt because coupling produces new error in the case that IMU has scale factor error and alignment error.

In above-mentioned each scheme, inner axle, outer annulate shaft are rotated with Constant Angular Velocity ω 1, ω 2 respectively, and the scope of ω 1 and ω 2 is 0.6 °/s--60 °/s.

Beneficial effects of the present invention：

The present invention is directed to the coarse alignment of the carrier being in small size low frequency sway state it is proposed that local solidify IMU coordinate system (p₀) concept, using based on p₀Posture renewal correction each cycle coarse alignment estimated value of coordinate system, such that it is able to continue through rotation Modulation system and the method averaged compensate the coarse alignment error that inertia device zero causes partially.Emulation experiment shows, is waving base Under the conditions of seat, although carrier waves, the interference signal that vibrates will weaken the error inhibitory action of computing of averaging, based on working as The rotary coarse alignment method of ground solidification IMU coordinate system remains to the error effectively suppressing inertia device zero partially to cause, thus improving Coarse alignment precision.

Brief description

Fig. 1 is swaying base coarse alignment emulation experiment misalignment schematic diagram.

Specific embodiment

When in practical application, the carrier such as naval vessel is in small size low frequency sway, oscillatory regime, attitude of carrier matrixNo longer For constant value, for solving this problem, introduce local solidification IMU coordinate system p₀It is correspondingly improved analytic method coarse alignment to design.

Local solidification IMU coordinate system p₀Definition be：The IMU with initial time under navigational coordinate system (geographic coordinate system) Coordinate system overlaps, and remains and geographic coordinate system geo-stationary, and that is, zero is overlapped with local geographic coordinate system n, with n The direction cosine matrix of system is constant value, and the synchronous coordinate system around geocentric inertial coordinate system i rotation with n system.

Averaging of coarse alignment should be directed to constant value attitude matrix, according to p under swaying base₀System's definition selects constant value square Battle arrayIn its method of counting of each sampled point it is：

${\hat{C}}_{p0}^n={\hat{C}}_p^n{C}_{p0}^p$

WhereinTry to achieve as the following formula：Wherein P be geographic coordinate system under gravity cause ratio force vector, Ball angular velocity vector and its matrix of multiplication cross vector composition, expression formula is：

$P=\left[\begin{array}{c}{\left(f_0^n\right)}^T\\ {\left({\mathrm{\ω}}_{\mathrm{ie}}^n\right)}^T\\ {\left(E^n\right)}^T\end{array}\right]$

$f_0^n={\left[\begin{array}{ccc}0& 0& g\end{array}\right]}^T$

${\mathrm{\ω}}_{\mathrm{ie}}^n={\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L& {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]}^T$

$E^n=f_0^n\×{\mathrm{\ω}}_{\mathrm{ie}}^n$

In formula, L is quiet base carrier place latitude.

Q is the matrix of these three vectors composition under IMU coordinate system, in the application with measured valueReplace：

$Q=\left[\begin{array}{c}{\left(f_0^p\right)}^T\\ {\left({\mathrm{\ω}}_{\mathrm{ie}}^p\right)}^T\\ {\left(E^p\right)}^T\end{array}\right]\≈\tilde{Q}=\left[\begin{array}{c}{\left({\tilde{f}}^p\right)}^T\\ {({\widetilde{\mathrm{\ω}}}_{\mathrm{ip}}^p-{\mathrm{\ω}}_{\mathrm{bp}}^p)}^T\\ {\left({\tilde{E}}^p\right)}^T\end{array}\right]$

In formulaThe measured value of the ratio force vector causing for gravity under IMU coordinate system；For gyroscope during coarse alignment Measured value, is earth rate vector and IMU angular velocity of rotation vector sum, therefore needs therefrom to deduct IMU angular velocity of rotation (can read from the rotating shaft angular-rate sensor of IMU rotating mechanism), the mathematical model of the two is：

${\tilde{f}}^p=f_0^p+\mathrm{\Δ}+w_a$

${\widetilde{\mathrm{\ω}}}_{\mathrm{ie}}^p={\mathrm{\ω}}_{\mathrm{ie}}^p+\mathrm{\ϵ}+w_g$

In formula, zero deflection amount of Δ and ε respectively accelerometer and gyro；W_aAnd W_gIt is respectively accelerometer and gyro Random noise vector (being assumed to be white noise).

According to p₀System's definition,Initial value be unit matrix, in each resolving cycle of coarse alignment, need according to gyro Output valve carries out posture renewal to it：

${\stackrel{\·}{C}}_p^{p0}=C_p^{p0}{\mathrm{\Ω}}_{p0,p}^p=C_p^{p0}{\mathrm{\Ω}}_{\mathrm{np}}^p$

${\mathrm{\ω}}_{\mathrm{np}}^p={\mathrm{\ω}}_{\mathrm{ip}}^p-C_n^p{\mathrm{\ω}}_{\mathrm{in}}^n$

$\begin{array}{c}{\mathrm{\ω}}_{\mathrm{np}}^p\left(t_{k+1}\right)={\widetilde{\mathrm{\ω}}}_{\mathrm{ip}}^p\left(t_{k+1}\right)-{\hat{C}}_n^p\left(t_k\right){\mathrm{\ω}}_{\mathrm{in}}^n\left(t_k\right)\\ ={\widetilde{\mathrm{\ω}}}_{\mathrm{ip}}^p\left(t_{k+1}\right)-{\hat{C}}_n^p\left(t_k\right){\mathrm{\ω}}_{\mathrm{ie}}^n\end{array}$

Wherein, due to p₀System and n system geo-stationary, and carrier ground velocity is approximately 0, therefore has

${\mathrm{\Ω}}_{p0,p}^p={\mathrm{\Ω}}_{\mathrm{np}}^p$

${\mathrm{\ω}}_{\mathrm{in}}^n\≈{\mathrm{\ω}}_{\mathrm{ie}}^n$

Utilize gyro to measure value in systemWhen updating IMU attitude matrix, because its angular movement dynamic range is higher than general Logical inertial navigation system being vibrated by rotating mechanism is affected, and therefore high-precision rotary inertial navigation needs to adopt rotating vector algorithm Or other innovatory algorithm suppression coning error, do not discuss in detail herein.

What each moment was solved treats stable constant value matrixAverage, and then can be in the hope of coarse alignment result：

${\stackrel{\‾}{C}}_{p0}^n=\frac{1}{N}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\hat{C}}_{p0}^n\left(t_k\right)$

${\hat{C}}_p^n\left(t_{\mathrm{fin}}\right)={\stackrel{\‾}{C}}_{p0}^n{C}_p^{p0}\left(t_{\mathrm{fin}}\right)$

T in formula_finFor coarse alignment finish time.

In the case of pedestal waves,The mathematical model of estimated value is：

$\begin{array}{c}{\hat{C}}_{p0}^n={\hat{C}}_p^n{C}_b^p{C}_{p0}^b\\ =P^{-1}\tilde{Q}{C}_b^p{C}_{p0}^b\\ =P^{-1}(Q+\mathrm{\δQ})C_b^p{C}_{p0}^b\\ =P^{-1}Q{C}_b^p{C}_{p0}^b+P^{-1}\mathrm{\δQ}{C}_b^p{C}_{p0}^b\\ =C_{p0}^n+\mathrm{\δ}{C}_{p0}^n\end{array}$

Treat stable constant value matrixThe equivalent integration forms of error be：

$\begin{array}{c}\frac{1}{N}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}\mathrm{\δ}{C}_{p0}^n\left(k\right)\\ =\frac{T_s}{T_c}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}\mathrm{\δ}{C}_{p0}^n\left(k\right)\\ \≈\frac{1}{T_c}{\∫}_{T_c}\mathrm{\δ}{C}_{p0}^n\mathrm{dt}\end{array}$

Because carrier angle of oscillation motion amplitude is much smaller than the angular movement amplitude selecting modulation, it is approximately considered during analytical error Have： $\mathrm{\δ}{C}_{p0}^n\≈P^{-1}\mathrm{\δQ}{C}_b^p$

Under swaying base, inertia device is output as：

${\tilde{g}}^p=f_0^p+a_D+\mathrm{\Δ}+w_a$

${\widetilde{\mathrm{\ω}}}_{\mathrm{ie}}^p={\mathrm{\ω}}_{\mathrm{ie}}^p+{\mathrm{\ω}}_D+\mathrm{\ϵ}+w_g$

A in formula_DFor carrier acceleration, ω_DWave angular velocity for carrier, be the interference signal of coarse alignment.

Now coarse alignment error is：

$\mathrm{\δQ}=\left[\begin{array}{c}{(a_D+\mathrm{\Δ}+w_a)}^T\\ {(\mathrm{\ϵ}+{\mathrm{\ω}}_D+w_g)}^T\\ {\left(\mathrm{\δ}{E}^p\right)}^T\end{array}\right]$

Wherein δ E^pExpression formula is：

Rotation modulation analytic method coarse alignment based on local solidification IMU coordinate system is inclined to inertia device zero under swaying base The error causing is inhibited, but waves and vibrate the mistake causing due to increased carrier in now inertia device measured value Difference component, averages to interference signal W_EInhibitory action declined.

Improved rotary swing pedestal analytic method can also be verified by emulation experiment to the improvement of coarse alignment.

Simulated conditions set as follows：Quiet base carrier place latitude is 30 ° of north latitude；Course is 30 ° of north by east；Accelerometer Zero is 10 partially^-4g；The inclined 0.02 °/h of gyro zero；Inertia device white noise standard deviation all takes zero inclined 1/2；Inertia device sample frequency For 0.05s.Carrier waves and is set to 0.002deg/s amplitude, the sinusoidal angular movement of 0.01Hz；Carrier vibration is set to 0.2m/s²Width Value, the sinusoidal line motion of 0.1Hz.

Carry out the emulation of two kinds of coarse alignment methods：A method that () is averaged to traditional analytic method, directly carries out strapdown The coarse alignment of system, averaged in simulation time finish time；(b) rotation modulation solution based on local solidification IMU coordinate system Analysis method, rotation approach continuously rotates for twin shaft, and each rotation changes direction of rotation, rotating shaft x_pWith z_pAngular speed than for 1：2.Often Emulation experiment coarse alignment time is 300s, in t=300s, respectively two methods is estimated in the coarse alignment in each sampling period Evaluation is averaged, as the final result of a coarse alignment.The appearance of each cycle posture renewal wherein in improved method coarse alignment State angle error is substituted using the attitude error that the emulation of navigation error equation obtains.The misalignment of each coarse alignment such as Fig. 1 institute Show.

In 100 coarse alignment experiments, the misalignment average absolute value of improved method is reduced to the 91.35% of traditional method, 18.83%, 67.51%.Experiment shows, can have using based on the rotary analytic method coarse alignment of local solidification IMU coordinate system Effect improves coarse alignment precision.

Claims (3)

1. the swaying base inertial navigation system coarse alignment method based on rotation modulation is it is characterised in that comprise the following steps：

The first step：The twin shaft making rotary inertia guiding systems during coarse alignment is periodically rotated by default rotation approach；

Second step：The attitude matrix estimated value of IMU is all obtained in each sampling period of IMU using analytic method；

${\hat{C}}_p^n=P^{-1}\tilde{Q};$

The square of ratio force vector, earth rate vector and its multiplication cross vector composition that wherein P causes for gravity under geographic coordinate system Gust, expression formula is：

$P=\left[\begin{array}{c}{\left(f_0^n\right)}^T\\ {\left({\mathrm{\ω}}_{ie}^n\right)}^T\\ {\left(E^n\right)}^T\end{array}\right]$

$f_0^n={\left[\begin{array}{ccc}0& 0& g\end{array}\right]}^T$

${\mathrm{\ω}}_{ie}^n={\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{ie}\cos L& {\mathrm{\ω}}_{ie}\sin L\end{array}\right]}^T$

$E^n=f_0^n\×{\mathrm{\ω}}_{ie}^n$

In formula, L is quiet base carrier place latitude；Q is the matrix of these three vectors composition under IMU coordinate system, in the application to survey ValueReplace：

$Q=\left[\begin{array}{c}{\left(f_0^p\right)}^T\\ {\left({\mathrm{\ω}}_{ie}^p\right)}^T\\ {\left(E^p\right)}^T\end{array}\right]\≈\tilde{Q}=\left[\begin{array}{c}{\left({\tilde{f}}^p\right)}^T\\ {({\widetilde{\mathrm{\ω}}}_{ip}^p-{\mathrm{\ω}}_{bp}^p)}^T\\ {\left({\tilde{E}}^p\right)}^T\end{array}\right]$

In formulaThe measured value of the ratio force vector causing for gravity under IMU coordinate system；Measure for gyroscope during coarse alignment Value, is earth rate vector and IMU angular velocity of rotation vector sum, need to therefrom deduct IMU angular velocity of rotation

3rd step：Each sampling period according to the angular velocity that gyro exports carry out attitude matrix update obtain current time from work as Ground solidification IMU coordinate is tied to the direction cosine matrix of IMU coordinate system；

${\stackrel{\·}{C}}_p^{p0}=C_p^{p0}{\mathrm{\Ω}}_{p0,p}^p=C_p^{p0}{\mathrm{\Ω}}_{np}^p$

${\mathrm{\ω}}_{np}^p={\mathrm{\ω}}_{ip}^p-C_n^p{\mathrm{\ω}}_{in}^n$

$\begin{array}{c}{\mathrm{\ω}}_{np}^p\left(t_{k+1}\right)={\widetilde{\mathrm{\ω}}}_{ip}^p\left(t_{k+1}\right)-{\hat{C}}_n^p\left(t_k\right){\mathrm{\ω}}_{in}^n\left(t_k\right)\\ ={\widetilde{\mathrm{\ω}}}_{ip}^p\left(t_{k+1}\right)-{\hat{C}}_n^p\left(t_k\right){\mathrm{\ω}}_{ie}^n\end{array}$

Antisymmetric matrix form for the angular velocity vector with respect to solidification inertial system for the IMU；For IMU with respect to geography The antisymmetric matrix form of the angular velocity vector of coordinate system；For geographic coordinate system with respect to inertial coodinate system angular velocity to Amount；T is the time of coarse alignment to current time, and subscript k represents the kth sampling period；

4th step：It is tied to IMU according to current time IMU attitude matrix estimated value and current time from local solidification IMU coordinate to sit Mark the constant value direction cosines that the direction cosine matrix being is obtained this sampling period and is tied to geographic coordinate system from local solidification IMU coordinate The estimated value of matrix；

${\hat{C}}_{p0}^n={\hat{C}}_p^n{C}_{p0}^p$

5th step：The constant value direction being tied to geographic coordinate system from local solidification IMU coordinate that each moment tries to achieve to coarse alignment process Cosine matrix is averaged, and obtains solidifying, from local, the final estimation that IMU coordinate is tied to the direction cosine matrix of geographic coordinate system Value；

${\stackrel{\‾}{C}}_{p0}^n=\frac{1}{N}\underset{k=1}{\overset{N}{\Σ}}{\hat{C}}_{p0}^n\left(t_k\right)$

The equal sign left side represents that local solidification IMU coordinate is tied to the meansigma methodss of the constant value direction cosine matrix of geographic coordinate system；Equal sign The right N represents and carries outThe total periodicity calculating, 1/N Σ represents it is averaged；Represent The local solidification IMU coordinate in kth cycle is tied to the direction cosine matrix of geographic coordinate system；

6th step：The constant value direction cosine matrix being tied to geographic coordinate system from local solidification IMU coordinate tried to achieve according to the 5th step It is multiplied by the direction cosines square being tied to local solidification IMU coordinate system from IMU coordinate that coarse alignment finish time obtains by posture renewal Battle array, that is, obtain the attitude matrix of coarse alignment finish time IMU；

${\hat{C}}_p^n\left(t_{fin}\right)={\stackrel{\‾}{C}}_{p0}^n{C}_p^{p0}\left(t_{fin}\right).$

2. the swaying base inertial navigation system coarse alignment method based on rotation modulation as claimed in claim 1 is it is characterised in that the In one step, default rotation approach includes following scheme：

A. inner axle, the unidirectional continuous rotation of outer annulate shaft；

B. inner axle, outer annulate shaft continuously rotate, and the change that often rotates a circle turns to；

C. inner axle, outer annulate shaft is unidirectional is alternately rotated, each axle rotates a circle, and stops starting simultaneously at another axle of rotation, so follows Ring is reciprocal；

D. inner axle, outer annulate shaft break-in are alternately rotated, and first axle stops after rotating a circle, and is then rotated a circle by the second axle, so Reversely rotated one week by first axle more afterwards, then reversely rotated one week by the second axle again, so move in circles；

E. inner axle, outer annulate shaft break-in are alternately rotated, and first axle reversely rotates one week after rotating a circle again, then stops, then Reversely rotate again after being rotated a circle by the second axle one week, so move in circles；

Wherein scheme a, c only can use in the case that the rotation platform of Rotating Inertial Navigation System contains conducting slip ring, and works as IMU can not adopt because coupling produces new error in the case of there is scale factor error and alignment error.

3. the swaying base inertial navigation system coarse alignment method based on rotation modulation as claimed in claim 2 it is characterised in that on State inner axle in each scheme, outer annulate shaft to rotate with Constant Angular Velocity ω 1, ω 2 respectively, the scope of ω 1 and ω 2 is 0.6 °/s-- 60°/s.