CN102519460A - Non-linear alignment method of strapdown inertial navigation system - Google Patents

Abstract

A non-linear alignment method of a strapdown inertial navigation system comprises the following steps of: acquiring data of an accelerometer and a fiber gyroscope of the fiber strapdown inertial navigation system and carrying out denoise processing, realizing coarse alignment and fine alignment processes by the utilization of an analysis method and a compass method, establishing a quaternion-based strapdown inertial navigation system non-linear error model under the condition that attitude angle and azimuth angle are both large misalignment angles, establishing an observation model with speed as observed quantity, carrying out iterative and filter estimation by the use of an improved UKF algorithm on the basis of the model so as to obtain the misaligned angle of the platform, continuously carrying out closed-loop feedback and correcting attitude matrix of the strapdown inertia system in the previous period, so as to accurately complete the initial alignment process. The method can be used to guarantee safety and confidentiality of the initial alignment without the utilization of other sensor information. By the introduction of the quaternion error based non-linear system error model, linearization is not required to guarantee the precision of the model. Computational complexity is reduced, and filtering is carried out on the established non-linear system so as to complete fine alignment.

Description

A kind of strapdown inertial navigation system non-linear alignment method

Technical field

The present invention relates to a kind of fast, high precision, passive strapdown inertial navigation system non-linear alignment method, be specially adapted to the big misalignment of strapdown inertial navigation system and aim at.

Background technology

In the inertial navigation initial alignment, because initial misalignment is bigger, generally need carry out coarse alignment earlier, utilize Kalman filter to carry out fine alignment again.Coarse alignment mainly is exactly to carry out horizontal aligument work, and this process also can comprise the orientation coarse alignment, and the coarse alignment precision is lower, and has prolonged the aligning time of initial alignment.In the mathematical model strictness of SINS is nonlinear model, and ψ equation and linear filter method commonly used can not accurately be explained, and needs to adopt accurate nonlinearity erron model and nonlinear filtering algorithm.For this reason; Numerous scholar's research can be applicable to the nonlinear model and the corresponding nonlinear filtering algorithm of wide-angle error, more and more widely simultaneously based on the application of non-linear filtering method (like EKF and UKF) in moving pedestal and flight are aimed at of big azimuthal error model.But existing technology is to the alignment case under the little misalignment mostly, also is more complicated on algorithm even be directed against big misalignment.

Summary of the invention

Technology of the present invention is dealt with problems and is: overcome the deficiency of prior art, provide a kind of fast, high precision, passive strapdown inertial navigation system non-linear alignment method, be specially adapted to the big misalignment of strapdown inertial navigation system and aim at.The present invention is directed to the initial alignment problem of SINS under big misalignment; Successively through analytical method coarse alignment, compass method fine alignment; And introduce NLS error model, do not make the precision that any linearization process guarantees model, on this basis based on the hypercomplex number error; Introducing speed amount has provided the nonlinear model that is applicable to big misalignment as measurement information; On the basis of classical UKF filtering, derive improved UKF filtering algorithm, avoid calculating the Jacobian matrix, the NLS of being set up is carried out filtering accomplish fine alignment according to corresponding condition.

Concrete technical scheme of the present invention is:

(1) under quiet pedestal condition, gathers the data of fiber strapdown inertial navigation system accelerometer and fibre optic gyroscope, and do denoising;

(2) utilize analytical method tentatively to accomplish the coarse alignment process of system, obtain the approximate down attitude matrix

of day coordinate system (ENU) northeastward

Wherein θ ₀And γ ₀Be respectively angle, initial heading, initial pitch angle and initial horizontal cradle angle.

(3) the compass method with platform compass is applied to strapdown inertial navigation system completion fine alignment phase one process, comprises horizontal fine alignment and orientation fine alignment, obtains attitude matrix

Corresponding course angle

Pitch angle θ ₁With roll angle γ ₁

(4) set up the strapdown inertial navigation system nonlinearity erron model based on hypercomplex number that is big misalignment to attitude angle and position angle, foundation is with the observation model of speed as observed quantity.

(5) the initial alignment nonlinear model according to step (4) improves nonlinear filtering algorithm UKF; With improved UKF the model of step (4) is carried out the iteration Filtering Estimation; Obtain the platform misalignment; Constantly the strap down inertial navigation system attitude matrix in last cycle of close-loop feedback correction obtains attitude matrix

thereby accomplish accurate initial alignment

Above-mentioned steps is further specified as follows:

1. described in the step (1) accelerometer and optical fibre gyro data are carried out the denoising method, adopt wavelet multiresolution rate analytical technology to carry out denoising, remove and be included in noise and the undesired signal in the high-frequency signal, improve the signal-to-noise characteristic of data.

2. the analytical method coarse alignment method described in the step (2) can adopt a kind of of following two kinds of methods, is of equal value:

(a) utilize the vectorial g of three mutually orthogonals, g * ω _Ie(g * ω _Ie) * g calculates

In order to calculate

Utilize the vectorial a of three mutually orthogonals ^b, a ^b* ω ^b(a ^b* ω ^b) * a ^bCalculate

$C_{b\left(0\right)}^{\′n}={\left[\begin{array}{c}{\left(g^n\right)}^T\\ {(g^n\×{\mathrm{\ω}}_{\mathrm{ie}}^n)}^T\\ {[(g^n\×{\mathrm{\ω}}_{\mathrm{ie}}^n)\×t^n]}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left(a^b\right)}^T\\ {(a^b\×{\mathrm{\ω}}^b)}^T\\ {[(a^b\×{\mathrm{\ω}}^b)\×a^b]}^T\end{array}\right]$

Wherein, g ⁿThe projection of expression acceleration of gravity under navigation coordinate system,

The projection of expression rotational-angular velocity of the earth under navigation coordinate system, a ^bExpression acceleration measuring value, ω ^bExpression gyroscope survey value, a ^b=g ^b+ δ a ^b, δ a ^bExpression adds the measuring error of table, δ ω ^bRepresent gyrostatic measuring error.

Attitude matrix

is carried out normalization to be handled:

$C_{b\left(0\right)}^n=C_{b\left(0\right)}^{\′n}{\left[{\left(C_{b\left(0\right)}^{\′n}\right)}^T{C}_{b\left(0\right)}^{\′n}\right]}^{-\frac{1}{2}}$

is exactly the attitude matrix that coarse alignment obtains;

(b) directly calculate

by measured value

For the ease of writing expression, the transition matrix

that navigation coordinate is tied to carrier coordinate system is write as:

$C_b^n=\left[\begin{array}{ccc}{C}_1& C_2& C_3\end{array}\right]$

C wherein ₁, C ₂, C ₃For constituting

Three column vectors, and have quadrature constraint to each other.

If the measured value of k moment gyroscope and accelerometer is designated as ω respectively ^b(k) and a ^b(k), adopt the least square method under the quadrature constraint to estimate

Three branch column vectors:

${\hat{C}}_3=-\frac{1}{\mathrm{\λ}}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)$

${\hat{C}}_2=\frac{1}{\mathrm{\η}}(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)+\mathrm{\ρ}\hat{z})$

${\hat{C}}_1=\hat{C}2\×\hat{C}3=\frac{1}{\mathrm{\η}}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\×{\hat{C}}_3$

Wherein, N is total measurement number of times,

$\mathrm{\λ}={\left[{\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)\right)\right]}^{\frac{1}{2}}$

$\mathrm{\ρ}=\frac{1}{\mathrm{\λ}}{\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{a}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\right)$

$\mathrm{\η}={[{\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}^b\left(k\right)\right)-{\mathrm{\ρ}}^2]}^{\frac{1}{2}}$

Then the estimated value of initial attitude matrix is:

$C_{b\left(0\right)}^n={\left[\begin{array}{ccc}{\hat{C}}_1& {\hat{C}}_2& {\hat{C}}_3\end{array}\right]}^T$

that calculate has been orthogonal matrix, do not need to do orthogonalization process again.

3. the foundation described in the step (4) is following to the strapdown inertial navigation system nonlinearity erron model based on hypercomplex number that attitude angle and position angle are big misalignment:

Attitude error equations:

$\mathrm{\&delta;}{\stackrel{\&CenterDot;}{Q}}_b^n=\frac{1}{2}<{\mathrm{\&omega;}}_{\mathrm{ib}}^b>\mathrm{\&delta;}{Q}_b^n-\frac{1}{2}\left[{\mathrm{\&omega;}}_{\mathrm{in}}^n\right]\mathrm{\&delta;}{Q}_b^n+\frac{1}{2}U\left({\hat{Q}}_b^n\right)({\mathrm{\&epsiv;}}^b+{\mathrm{\&omega;}}_g^b)-\frac{1}{2}Y\left({\hat{Q}}_b^n\right)\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{en}}^n$

Be the linear differential equation of hypercomplex number, in the formula, Be the error of hypercomplex number, $<{\mathrm{\&omega;}}_{\mathrm{Ib}}^b>=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_x& -{\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_z\\ {\mathrm{\&omega;}}_x& 0& {\mathrm{\&omega;}}_z& -{\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_z& 0& {\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_z& {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_x& 0\end{array}\right],$

ω wherein _x, ω _y, ω _zBe respectively following three the direction gyro angular speeds of carrier coordinate system, $\left[{\mathrm{\&omega;}}_{\mathrm{In}}^n\right]=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_E& -{\mathrm{\&omega;}}_N& -{\mathrm{\&omega;}}_U\\ {\mathrm{\&omega;}}_E& 0& -{\mathrm{\&omega;}}_U& {\mathrm{\&omega;}}_N\\ {\mathrm{\&omega;}}_N& {\mathrm{\&omega;}}_U& 0& -{\mathrm{\&omega;}}_E\\ {\mathrm{\&omega;}}_U& -{\mathrm{\&omega;}}_N& {\mathrm{\&omega;}}_E& 0\end{array}\right],$

ω wherein _E, ω _N, ω _UBe respectively navigation coordinate system (ENU) following three direction gyro angular speeds,

$U\left({\hat{Q}}_b^n\right)\stackrel{\mathrm{\&Delta;}}{=}U=\left[\begin{array}{ccc}-{\hat{q}}_1& -{\hat{q}}_2& -{\hat{q}}_3\\ {\hat{q}}_0& -{\hat{q}}_3& {\hat{q}}_2\\ {\hat{q}}_3& {\hat{q}}_0& -{\hat{q}}_1\\ -{\hat{q}}_2& {\hat{q}}_1& {\hat{q}}_0\end{array}\right],$ $Y\left({\hat{Q}}_b^n\right)\stackrel{\mathrm{\&Delta;}}{=}Y=\left[\begin{array}{ccc}-{\hat{q}}_1& -{\hat{q}}_2& -{\hat{q}}_3\\ {\hat{q}}_0& {\hat{q}}_3& -{\hat{q}}_2\\ -{\hat{q}}_3& {\hat{q}}_0& {\hat{q}}_1\\ {\hat{q}}_2& -{\hat{q}}_1& {\hat{q}}_0\end{array}\right],$

[q wherein ₀, q ₁, q ₂, q ₃] be hypercomplex number,

Be the projection of gyro angular velocity under carrier coordinate system, ε ^bBe gyroscopic drift, Be corresponding gyro to measure white noise,

The expression navigation coordinate is the projection of angular velocity under navigation system of spherical coordinate system relatively, in whole derivation, does not suppose that misalignment is that therefore, this equation can be described carrier and be the attitude error propagation characteristic under the big misalignment three attitudes in a small amount;

The velocity error equation:

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{V}=-2\left[{\hat{C}}_b^n{\hat{f}}^b\right]\&times;Y^T\left(\hat{Q}\right)\mathrm{\&delta;Q}+2{\hat{C}}_b^n{\hat{f}}^b{\hat{Q}}^T\mathrm{\&delta;Q}-Y^T\left(\mathrm{\&delta;Q}\right)U\left(\mathrm{\&delta;Q}\right){\hat{f}}^b+C_b^n({\&dtri;}^b+{\mathrm{\&omega;}}_a^b)-({\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{in}}^n)\&times;\mathrm{\&delta;V}-\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{en}}^n\&times;V^n$

In the formula,

Be the calculated value of attitude battle array,

Be acceleration measuring value, ▽ ^bBe the accelerometer biasing,

Be the accelerometer measures white noise, δ g ⁿBe the projection of gravity acceleration error under navigation coordinate system;

The site error equation:

$\left[\begin{array}{c}\mathrm{\&delta;}\stackrel{\&CenterDot;}{L}\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{h}\end{array}\right]=\left[\begin{array}{ccc}0& 0& -\stackrel{\&CenterDot;}{L}/(R_M+h)\\ \stackrel{\&CenterDot;}{\mathrm{\&lambda;}}\tan \mathrm{\&lambda;}& 0& -\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}/(R_N+h)\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;L}\\ \mathrm{\&delta;\&lambda;}\\ \mathrm{\&delta;h}\end{array}\right]+\left[\begin{array}{ccc}0& 1/(R_M+h)& 0\\ \sec L/(R_N+h)& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;}{v}_E\\ \mathrm{\&delta;}{v}_N\\ \mathrm{\&delta;}{v}_U\end{array}\right]$

In the formula, δ L, δ λ, δ h is respectively latitude error, longitude error and height error, R _MBe the radius-of-curvature in the reference ellipsoid meridian ellipse, R _M=R _e(1-2e+3e sin ²L), R _NBe the radius-of-curvature in the plane normal of vertical meridian ellipse, R _N=R _e(1+e sin ²L), R wherein _eBe the major axis radius of reference ellipsoid, e is the ovality of ellipsoid;

State vector is got $x=\left[\mathrm{\&delta;}{V}_E\mathrm{\&delta;}{V}_N\mathrm{\&delta;}{V}_U\mathrm{\&delta;}{q}_0\mathrm{\&delta;}{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000117862560000021.png,HDA0000117862560000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_1\mathrm{\&delta;}{q}_2\mathrm{\&delta;}{q}_3{\&dtri;}_x^b{\&dtri;}_y^b{\&dtri;}_z^b{\mathrm{\&epsiv;}}_x^b{\mathrm{\&epsiv;}}_y^b{\mathrm{\&epsiv;}}_z^b\right],$ Noise vector is got

Set up the filter state model, and with velocity error Z=δ v=[δ v _x, δ v _y] ^TFor observation equation is set up in observed quantity:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{X}=f\left(X\right)+g\left(X\right)W\\ Z=\mathrm{HX}+V\end{array}\right.$

Wherein, H=[I _{3 * 3}0 _{3 * 10}], V is for measuring noise.

Described in the step (5) when measuring equation and be linear equation, improved UKF filtering method is following:

$\left\{\begin{array}{c}{\mathrm{\&chi;}}_{k-1}=[{\hat{x}}_{k-1}{\left[x_k\right]}_L\&PlusMinus;\mathrm{\&gamma;}\sqrt{P_{k-1}}]\\ {\mathrm{\&chi;}}_{i,k|k-1}^{*}=f\left({\mathrm{\&chi;}}_{i,k-1}\right)\\ {\hat{x}}_{k|k-1}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^{\left(m\right){\mathrm{\&chi;}}_{i,k|k-1}^{*}}\\ P_{k|k-1}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}{({\mathrm{\&chi;}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1})({\mathrm{\&chi;}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1})}^T+g\left({\hat{x}}_{k-1}\right)Q_{k-1}g{\left({\hat{x}}_{k-1}\right)}^T\\ P_{{\stackrel{\&CenterDot;}{x}}_k{\stackrel{\&CenterDot;}{z}}_k}=P_{k|k-1}{H}_k^T,P_{{\stackrel{\&CenterDot;}{z}}_k{\stackrel{\&CenterDot;}{z}}_k}=H_k{P}_{k|k-1}{H}_k^T+R_k\\ K_k=P_{{\stackrel{\&CenterDot;}{x}}_k{\stackrel{\&CenterDot;}{z}}_k}{P}_{{\stackrel{\&CenterDot;}{z}}_k{\stackrel{\&CenterDot;}{z}}_k}^{-1}\\ {\hat{z}}_{k|k-1}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{H}_k{\hat{x}}_{k|k-1}\\ {\hat{x}}_k={\hat{x}}_{k|k-1}+k_k(z_k-{\hat{z}}_{k|k-1})\\ P_k=P_{k|k-1}-K_k{P}_{{\stackrel{\&CenterDot;}{z}}_k{\stackrel{\&CenterDot;}{z}}_k}{K}_k^T\end{array}\right.$

In the formula, χ _K-1Be k-1 sigma sampled point constantly,

Be k-1 state estimation constantly,

Represent a matrix, all column vectors are all by L x _kForm P _kBe the state variance matrix,

One-step prediction sigma sampled point,

Be state one-step prediction, K _kBe gain matrix,

For measuring the one-step prediction average, all the other calculation of parameter are following:

$\left\{\begin{array}{c}\mathrm{\&lambda;}={\mathrm{\&alpha;}}^2(L+\mathrm{\&kappa;})-L\\ \mathrm{\&gamma;}=\sqrt{L+\mathrm{\&lambda;}}\\ W_0^{\left(m\right)}=\mathrm{\&lambda;}/(L+\mathrm{\&lambda;}),W_0^{\left(c\right)}=\mathrm{\&lambda;}/(L+\mathrm{\&lambda;})+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})\\ W_i^{\left(m\right)}=W_i^{\left(c\right)}=1/\left[2(L+\mathrm{\&lambda;})\right],i(=\mathrm{1,2},...,2L)\end{array}\right.$

Utilize above-mentioned improved UKF filtering algorithm to obtain hypercomplex number variable quantity δ Q, convert platform misalignment matrix into thus constantly revising the initial matrix that step (3) obtains obtains the final initial alignment attitude matrix that satisfies precision:

$C_{b\left(2\right)}^n={\left(C_n^{n^{\&prime;}}\right)}^T{C}_{b\left(1\right)}^n$

Principle of the present invention: gather strapdown inertial navigation system accelerometer and gyro data; Do the small echo denoising; Utilizing analytical method to carry out coarse alignment obtains initial attitude matrix

and then utilizes the compass method to accomplish fine alignment further to obtain attitude matrix

and set up the nonlinear model to big misalignment; Utilize improved UKF algorithm that model is carried out Filtering Estimation and obtain platform misalignment matrix, obtain attitude matrix and accomplish the high precision initial alignment process thereby revised.

The present invention's advantage compared with prior art is:

(1) the present invention utilizes analytical method and compass method to accomplish initial alignment through gathering gyroscope and accelerometer data, and the method need not to utilize other sensor informations, guarantees the security and the confidentiality of initial alignment.

(2) the present invention is directed to the initial alignment problem of strapdown inertial navigation system under big misalignment; Introducing is based on the NLS error model of hypercomplex number error; Do not make the precision that any linearization process guarantees model, and be applicable to the initial alignment under big misalignment and the little misalignment.

(3) the present invention sets up improved UKF filtering algorithm according to nonlinear model on the basis of classical UKF filtering, lowers computation complexity thereby avoid calculating the Jacobian matrix, the NLS of being set up is carried out filtering accomplish fine alignment.

The present invention is to provide a kind of fast, high precision, passive strapdown inertial navigation system non-linear alignment method.The present invention through gathering fiber strapdown inertial navigation system accelerometer and fibre optic gyroscope data and do denoising; Utilize analytical method and compass method to realize coarse alignment and fine alignment process; Then foundation is the strapdown inertial navigation system nonlinearity erron model based on hypercomplex number of big misalignment to attitude angle and position angle; Foundation is carried out iteration Filtering Estimation with improved UKF algorithm with the observation model of speed as observed quantity on this model basis, obtain the platform misalignment; The strap down inertial navigation system attitude matrix in continuous last cycle of close-loop feedback correction, thus accurate initial alignment process accomplished.

The present invention utilizes analytical method and compass method to accomplish initial alignment through gathering gyroscope and accelerometer data, and the method need not to utilize other sensor informations, guarantees the security and the confidentiality of initial alignment; To the initial alignment problem of strapdown inertial navigation system under big misalignment; Introducing is based on the NLS error model of hypercomplex number error; Do not make the precision that any linearization process guarantees model, and be applicable to the initial alignment under big misalignment and the little misalignment; On the basis of classical UKF filtering, set up improved UKF filtering algorithm, lower computation complexity, the NLS of being set up is carried out filtering accomplish fine alignment thereby avoid calculating the Jacobian matrix according to nonlinear model.

Description of drawings

Fig. 1 is a SINS initial alignment process flow diagram;

Fig. 2 is a compass method east orientation channel error schematic diagram;

Fig. 3 is a compass method north orientation channel error schematic diagram;

Fig. 4 is a horizontal fine alignment loop east orientation passage schematic diagram;

Fig. 5 is a horizontal fine alignment loop north orientation passage schematic diagram;

Fig. 6 is an orientation fine alignment schematic diagram;

Fig. 7 (a)～Fig. 7 (c) is little misalignment initial alignment simulation curve;

Fig. 7 (a)～(c) is respectively pitch angle, roll angle and the course angle phantom error curve of simplifying UKF, conventional kalman and EKF filtering;

Fig. 8 (a)～Fig. 8 (c) is big misalignment initial alignment simulation curve.

It is bent that Fig. 8 (a)～(c) is respectively pitch angle, roll angle and the course angle phantom error of simplifying UKF, conventional kalman and EKF filtering.

Embodiment

Below in conjunction with accompanying drawing the present invention is explained further details.

(1) analytical method coarse alignment of the present invention carries out according to following method:

Coarse alignment is exactly the estimated value

of setting up an approximate attitude matrix thereby makes system can carry out next step fine alignment.The process of coarse alignment is mainly the process that the orientation sets: by the instantaneous course of carrier and the estimated value of attitude angle, obtain the estimated value of strapdown matrix

Local latitude L is a known quantity, and navigation system adopts sky, northeast coordinate system, then gravity acceleration g and rotational-angular velocity of the earth ω _IeComponent in navigation system all is definite known, can be expressed as:

g ⁿ＝[0?0?-g] ^T

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

Wherein Ω is the earth rotation angular speed, and g is the amplitude of acceleration of gravity.

Resolving coarse alignment is exactly to utilize known g ⁿ, With to g ^b,

Measured value estimate

Because the coarse alignment time is shorter, adopts usually the measurement value sensor equal way of making even is improved alignment precision.Write for expression, use a ^bWith

Represent accelerometer and gyrostatic average value measured respectively.

Analytical method coarse alignment method described in the step (2) can adopt a kind of of following two kinds of methods, is of equal value:

(a) utilize the vectorial g of three mutually orthogonals, g * ω _Ie(g * ω _Ie) * g calculates

In order to calculate

Utilize the vectorial a of three mutually orthogonals ^b, a ^b* ω ^b(a ^b* ω ^b) * a ^bCalculate

$C_{b\left(0\right)}^{\&prime;n}={\left[\begin{array}{c}{\left(g^n\right)}^T\\ {(g^n\&times;{\mathrm{\&omega;}}_{\mathrm{ie}}^n)}^T\\ {[(g^n\&times;{\mathrm{\&omega;}}_{\mathrm{ie}}^n)\&times;g^n]}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left(a^b\right)}^T\\ {(a^b\&times;{\mathrm{\&omega;}}^b)}^T\\ {[(a^b\&times;{\mathrm{\&omega;}}^b)\&times;a^b]}^T\end{array}\right]$

Wherein, g ⁿThe projection of expression acceleration of gravity under navigation coordinate system,

The projection of expression rotational-angular velocity of the earth under navigation coordinate system, a ^bExpression acceleration measuring value, ω ^bExpression gyroscope survey value, a ^b=g ^b+ δ a ^b,

δ a ^bExpression adds the measuring error of table, δ ω ^bRepresent gyrostatic measuring error,

Attitude matrix

is carried out normalization to be handled:

$C_{b\left(0\right)}^n=C_{b\left(0\right)}^{\&prime;n}{\left[{\left(C_{b\left(0\right)}^{\&prime;n}\right)}^T{C}_{b\left(0\right)}^{\&prime;n}\right]}^{-\frac{1}{2}}$

is exactly the attitude matrix that coarse alignment obtains;

(b) directly calculate

by measured value

For the ease of writing expression, the transition matrix

that navigation coordinate is tied to carrier coordinate system is write as:

$C_b^n=\left[\begin{array}{ccc}{C}_1& C_2& C_3\end{array}\right]$

C wherein ₁, C ₂, C ₃For constituting Three column vectors, and have quadrature constraint to each other.

If the measured value of k moment gyroscope and accelerometer is designated as ω respectively ^b(k) and a ^b(k), adopt the least square method under the quadrature constraint to estimate

Three branch column vectors:

${\hat{C}}_3=-\frac{1}{\mathrm{\&lambda;}}\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{a}^b\left(k\right)$

${\hat{C}}_2=\frac{1}{\mathrm{\&eta;}}(\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}^b\left(k\right)+\mathrm{\&rho;}\hat{z})$

${\hat{C}}_1=\hat{C}2\&times;\hat{C}3=\frac{1}{\mathrm{\&eta;}}\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}^b\left(k\right)\&times;{\hat{C}}_3$

Wherein, N is total measurement number of times,

$\mathrm{\&lambda;}={\left[{\left(\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{a}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{a}^b\left(k\right)\right)\right]}^{\frac{1}{2}}$

$\mathrm{\&rho;}=\frac{1}{\mathrm{\&lambda;}}{\left(\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{a}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}^b\left(k\right)\right)$

$\mathrm{\&eta;}={[{\left(\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}^b\left(k\right)\right)-{\mathrm{\&rho;}}^2]}^{\frac{1}{2}}$

Then the estimated value of initial attitude matrix is:

$C_{b\left(0\right)}^n={\left[\begin{array}{ccc}{\hat{C}}_1& {\hat{C}}_2& {\hat{C}}_3\end{array}\right]}^T$

that calculate has been orthogonal matrix, do not need to do orthogonalization process again.

(2) the compass method fine alignment method described in the present invention is following:

Can obtain a mathematical analysis platform

but that its precision can't reach system is required through coarse alignment, also need pass through fine alignment and carry out more high-precision aligning and leveling.The fine alignment process was divided into for two steps usually, at first was horizontal leveling, then was alignment of orientation.Alignment of orientation is based upon on the basis of horizontal aligument, generally adopts the compass bearing alignment methods.It is exactly to utilize compass effect item φ that so-called compass method is aimed at _zω _IeThe δ V that cosL causes _N, make φ with the method for circuit feedback _zBe decreased to ultimate value gradually.

(a) horizontal aligument loop

Coarse alignment finishes the error angle φ of back system _x, φ _y, φ _zAll can be considered little angle, φ _xAnd φ _yBetween cross-couplings can ignore, this moment the horizontal channel error equation be:

$\left[\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_E=-{\mathrm{\&phi;}}_{y}g+{\&dtri;}_E\\ {\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_y=\frac{\mathrm{\&delta;}{V}_E}{R}+{\mathrm{\&epsiv;}}_N\end{array}\right]$

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_N={\mathrm{\&phi;}}_{x}g+{\&dtri;}_N\\ {\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_x=-\frac{\mathrm{\&delta;}{V}_N}{R}-{\mathrm{\&phi;}}_z{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+{\mathrm{\&epsiv;}}_E\end{array}\right.$

By following formula can draw east orientation passage and north orientation channel error calcspar, as shown in Figures 2 and 3.

Can find out that by Fig. 2 and Fig. 3 two passages all are second-order system, and all are the shura loops, φ _xAnd φ _yDo undamped oscillation, be 84.4 minutes oscillation period, shows as mathematical platform Element do periodic variation.

In the horizontal circuit of inertial navigation system, introduce damping k ₁, k ₂, make quasi loop improved oscillation frequency.Damping information is taken from intrasystem velocity error information, after the damping network in the adding system.After introducing damping, the horizontal fine alignment schematic diagram of single channel such as Fig. 4 and shown in Figure 5, this moment, system rose to three rank.

As can be seen from the figure, work as k ₁, k ₂, k ₃When being zero, system is the second order undamped system, φ _xAnd φ _yThe angle is done unattenuated vibration with the shura cycle around zero point, and does not receive acceleration action, and its secular equation is:

$s^2+{\mathrm{\&omega;}}_s^2=0$

The control principle of above-mentioned three contrasts square case is: k is set ₁Negative feedback links, its input signal adopts δ V _cN, and output signal k ₁δ V _CNBe added to the output terminal of accelerometer, constitute the negative feedback of integral element, equivalence is inertial element 1/ (s+k ₁), at this moment, the secular equation of system is:

$s^2+k_{1}s+{\mathrm{\&omega;}}_s^2=0$

Can find out that from secular equation system has increased damping term k ₁S, but the natural mode shape of system does not become, and still is shura cycle, i.e. 84.4min.When misalignment was big, parsing platform fine alignment was longer to the time of horizontality, can not satisfy the requirement of initial alignment rapidity, in order to shorten the oscillation period of system, should increase in the system along the parallelly connected link k of feedback ₂/ R, the secular equation of system is like this:

$s^2+k_{1}s+(k_2+1){\mathrm{\&omega;}}_s^2=0$

Can see, increase k ₂Afterwards, system's built-in oscillation cycle shortens

Doubly, be still typical second-order system, can satisfy the requirement of dynamic perfromance equally, the system of this moment is that the second order damping system (has only k ₁The time system be called the single order damping system), through changing k ₂Can control the length of oscillation period.

Must eliminate by gyroscope constant value drift and the caused horizontal steady-state error of accelerometer bias, so should increase integral element, i.e. energy storage link at the gyro input end.As δ V _CN=0, produce a signal cancellation error source φ _zω _iEcosL+ ε _xThereby, eliminate constant error.Increase integral element k ₃Horizontal fine alignment loop behind the/sR is as shown in Figure 4, and this moment, system became three rank damping systems, and its secular equation is:

$s^3+k_1{s}^2+(1+k_2){\mathrm{\&omega;}}_s^2+g{k}_3=0$

Adopt said system leveling scheme,, just can make mathematical analysis platform and error angle φ within a certain period of time as long as choose suitable parameter _x, φ _yBe stabilized near the zero-bit.

If according to rapidity requirement, be σ to the attenuation coefficient of quasi loop, the damping natural frequency of vibration is ω _d, then the characteristic root of third-order system should be s ₁=-σ, s ₂=-σ+j ω _d, s ₃=-σ-j ω _d

So secular equation is:

$s^3+3\mathrm{\&sigma;}{s}^2+(3{\mathrm{\&sigma;}}^2+{\mathrm{\&omega;}}_d^2)s+{\mathrm{\&sigma;}}^3+\mathrm{\&sigma;}{\mathrm{\&omega;}}_d^2-(s+\mathrm{\&sigma;})(s+\mathrm{\&sigma;}-j{\mathrm{\&omega;}}_d)(s+\mathrm{\&sigma;}+j{\mathrm{\&omega;}}_d)=0$

Coefficient of comparisons,

k ₁＝3σ

$k_2=\frac{3{\mathrm{\&sigma;}}^2+{\mathrm{\&omega;}}_d^2}{{\mathrm{\&omega;}}_s^2}-1$

$k3=\frac{{\mathrm{\&sigma;}}^3+\mathrm{\&sigma;}{\mathrm{\&omega;}}_d^2}{g}$

If the damping ratio that known system requires is ξ, attenuation coefficient is σ, then

k ₁＝3σ

$k_2=\frac{{\mathrm{\&sigma;}}^2}{{\mathrm{\&omega;}}_s^2}(2+\frac{1}{{\mathrm{\&xi;}}^2})-1$

$k3=\frac{{\mathrm{\&sigma;}}^3}{g{\mathrm{\&xi;}}^2}$

System under underdamping state, when equivalent damping ratio ξ between 0～0.707, the dynamic process of misalignment correction from initial value begin rapidly to zero near, one or many passes zero point, and is stabilized near zero point gradually, thereby obtains more accurate strapdown matrix.

(b) alignment of orientation loop

The initial orientation fine alignment method that SINS is commonly used is introduced " mathematical platform " and is utilized closed loop gyrocompass effect principle to carry out afterwards.As shown in Figures 2 and 3, in horizontal aligument north orientation passage (forming), contain equivalent gyroscopic drift item φ by north orientation accelerometer and east orientation gyro _zω _IeCosL.Corresponding horizontal aligument east orientation passage (being made up of east orientation accelerometer and north gyro appearance) does not then have this; This explanation north orientation horizontal fine alignment loop and azimuth axis have confidential relation; Cross-couplings influence therebetween is bigger, therefore just might realize the orientation fine alignment.Here φ _zω _IeThe influence of cosL is called the compass effect, and φ is passed through by the loop, orientation exactly in so-called compass loop _zω _IeCosL is coupled to azimuth information in the north orientation horizontal circuit and forms.

When coordinates computed is that n ' is that n exists azimuthal error angle φ with navigation coordinate _zThe time, the gyroscope on the carrier will be experienced the component that rotational-angular velocity of the earth at n ' is, and this component makes n ' with n system declination angle arranged _xThis is equivalent to introduce φ from accelerometer _xG information produces velocity error δ V through integration _N, through measuring δ V _NCan measure the size of compass effect.

Because through angle φ _xCould be at δ V _NTherefore the size of middle reflection compass effect no longer repeats to be provided with integral element k in horizontal circuit ₃/ (sR) eliminate by ε _EAnd φ _zω _IeThe φ that cosL causes _xSteady-state error, but with δ V _NBe control signal, design a link and control φ _zThe angle makes it be reduced to the scope of permission.Want control azimuth error angle φ _z, must control ω with certain rules _CDDesign a controlling unit k (s), its input signal is δ V _N, the output signal is k (s) δ V _NThe φ from the angle _zBeginning through each link of compass effects, is arrived δ V at last _NOutput is passed through direction control link k (s), again up to output angle φ _zTill, such loop is called the compass loop.Utilize the compass loop to realize that the schematic diagram of orientation fine alignment is as shown in Figure 6.

The secular equation that can obtain the alignment of orientation loop according to Fig. 6 is:

$s^4+({\mathrm{<figure-callout\; id="1"\; label="K"\; filenames="HDA0000117862560000021.png,HDA0000117862560000022.png"\; state="\{\{state\}\}">K</figure-callout>}}_1+K_4)s^3+[{\mathrm{\&omega;}}_s^2(1+K_2)+K_1{K}_4]s^2+{\mathrm{\&omega;}}_s^2(1+K_2)K_{4}s+g{K}_3=0$

If according to dynamic response requirement; Damping ratio is

and establish attenuation coefficient is σ; Then damped oscillation frequency is ω d=σ, so characteristic root is s1, and 2=-σ-j σ; S3,4=-σ+j σ

Secular equation is:

s ⁴+4σs ³+8σ ²s ²+8σ ³s+4σ ⁴＝0

Relatively two formulas get

$\left\{\begin{array}{c}{\mathrm{<figure-callout\; id="1"\; label="K"\; filenames="HDA0000117862560000021.png,HDA0000117862560000022.png"\; state="\{\{state\}\}">K</figure-callout>}}_1+K_4=4\mathrm{\&sigma;}\\ {\mathrm{\&omega;}}_s^2(1+K_2)+K_1{K}_4=8{\mathrm{\&sigma;}}^2\\ {\mathrm{\&omega;}}_s^2(1+k_2)K_4=8{\mathrm{\&sigma;}}^3\\ g{k}_3=4{\mathrm{\&sigma;}}^4\end{array}\right.$

This is the non-linear algebraic equation about unknown parameter, and multi-solution is arranged, and chooses K ₁=K ₄, then get unique solution:

K ₁＝K ₄＝2σ

$K_2=\frac{4{\mathrm{\&sigma;}}^2}{{\mathrm{\&omega;}}_s^2}-1$

$K_3=\frac{4{\mathrm{\&sigma;}}^4}{g}$

(3) foundation described in the present invention is following to the strapdown inertial navigation system nonlinear model based on hypercomplex number that attitude angle and position angle are big misalignment:

Attitude error equations:

Carrier coordinate system with respect to the differential equation of the rotation hypercomplex number

of navigation coordinate system is:

${\stackrel{\&CenterDot;}{Q}}_b^n=\frac{1}{2}\left[Q_b^n\right]{\mathrm{\&omega;}}_{\mathrm{nb}}^b=\frac{1}{2}\left[Q_b^n\right]({\mathrm{\&omega;}}_{\mathrm{ib}}^b-{\mathrm{\&omega;}}_{\mathrm{in}}^b)=\frac{1}{2}<{\mathrm{\&omega;}}_{\mathrm{ib}}^b>Q_b^n-\frac{1}{2}\left[Q_b^n\right]\left({\left[Q_b^n\right]}^{-1}{\mathrm{\&omega;}}_{\mathrm{in}}^n\right[Q_b^n\left]\right)=\frac{1}{2}<{\mathrm{\&omega;}}_{\mathrm{ib}}^b>Q_b^n-\frac{1}{2}\left[{\mathrm{\&omega;}}_{\mathrm{in}}^n\right]Q_b^n$

Wherein:

$\left[Q_b^n\right]=\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right],$ $<{\mathrm{\&omega;}}_{\mathrm{ib}}^b>=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_x& -{\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_z\\ {\mathrm{\&omega;}}_x& 0& {\mathrm{\&omega;}}_z& -{\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_z& 0& {\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_z& {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_x& 0\end{array}\right],$

$\left[{\mathrm{\&omega;}}_{\mathrm{in}}^n\right]=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_E& -{\mathrm{\&omega;}}_N& -{\mathrm{\&omega;}}_N\\ {\mathrm{\&omega;}}_E& 0& -{\mathrm{\&omega;}}_U& {\mathrm{\&omega;}}_N\\ {\mathrm{\&omega;}}_N& {\mathrm{\&omega;}}_U& 0& -{\mathrm{\&omega;}}_E\\ {\mathrm{\&omega;}}_U& -{\mathrm{\&omega;}}_N& {\mathrm{\&omega;}}_E& 0\end{array}\right]$

The differential equation that obtains calculating hypercomplex number is:

${\stackrel{\&CenterDot;}{\hat{Q}}}_b^n=\frac{1}{2}<{\widehat{\mathrm{\&omega;}}}_{\mathrm{ib}}^b>{\hat{Q}}_b^n-\frac{1}{2}\left[{\widehat{\mathrm{\&omega;}}}_{\mathrm{in}}^n\right]{\hat{Q}}_b^n$

is the measured value of gyro in the formula, and is the measuring error of gyro; The n system that

expression calculates is with respect to the angular velocity of i system.

Two formulas are subtracted each other the attitude of carrier error equation of can handy additivity hypercomplex number error AQE representing:

$\mathrm{\&delta;}{\stackrel{\&CenterDot;}{Q}}_b^n=\frac{1}{2}<{\mathrm{\&omega;}}_{\mathrm{ib}}^b>\mathrm{\&delta;}{Q}_b^n-\frac{1}{2}\left[{\mathrm{\&omega;}}_{\mathrm{in}}^n\right]\mathrm{\&delta;}{Q}_b^n+\frac{1}{2}<\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{ib}}^b>{\hat{Q}}_b^n-\frac{1}{2}\left[\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{in}}^n\right]{\hat{Q}}_b^n$

Order $<\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{Ib}}^b>{\hat{Q}}_b^n=U\left({\hat{Q}}_b^n\right)\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{Ib}}^b,$ $\left[\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{In}}^n\right]{\hat{Q}}_b^n=Y\left({\hat{Q}}_b^n\right)\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{In}}^n,$ In the formula, U, Y define as follows

$U\left({\hat{Q}}_b^n\right)\stackrel{\mathrm{\&Delta;}}{=}U=\left[\begin{array}{ccc}-{\hat{q}}_1& -{\hat{q}}_2& -{\hat{q}}_3\\ {\hat{q}}_0& {\hat{q}}_3& {\hat{q}}_2\\ {\hat{q}}_3& {\hat{q}}_0& -{\hat{q}}_1\\ -{\hat{q}}_2& {\hat{q}}_1& {\hat{q}}_0\end{array}\right]Y\left({\hat{Q}}_b^n\right)\stackrel{\mathrm{\&Delta;}}{=}Y=\left[\begin{array}{ccc}-{\hat{q}}_1& -{\hat{q}}_2& -{\hat{q}}_3\\ {\hat{q}}_0& {\hat{q}}_3& -{\hat{q}}_2\\ -{\hat{q}}_3& {\hat{q}}_0& {\hat{q}}_1\\ {\hat{q}}_2& -{\hat{q}}_1& {\hat{q}}_0\end{array}\right]$

Provable U ^TU=I, Then:

$\mathrm{\&delta;}{\stackrel{\&CenterDot;}{Q}}_b^n=\frac{1}{2}<{\mathrm{\&omega;}}_{\mathrm{ib}}^b>\mathrm{\&delta;}{Q}_b^n-\frac{1}{2}\left[{\mathrm{\&omega;}}_{\mathrm{in}}^n\right]\mathrm{\&delta;}{Q}_b^n+\frac{1}{2}U\left({\hat{Q}}_b^n\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{ib}}^b\right)-\frac{1}{2}Y\left({\hat{Q}}_b^n\right)\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{in}}^n$

Following formula is the linear differential equation of hypercomplex number, in whole derivation, does not suppose that misalignment is that therefore, this equation can be described carrier and be the attitude error propagation characteristic under the big misalignment three attitudes in a small amount.

The velocity error equation:

The velocity error equation can pass through the disturbance acquisition of inertial navigation fundamental equation in n system,

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{V}=\mathrm{\&Delta;}{C}_b^n{\hat{f}}^b+C_b^n{\&dtri;}^b-({\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{i\hat{n}}^n)\&times;\mathrm{\&delta;V}-(2\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{ie}}^n+\mathrm{\&delta;}{\mathrm{\&omega;}}_{e\hat{n}}^n)\&times;V^n+\mathrm{\&delta;}{g}^n$

is the error of calculation of attitude battle array in the formula, and therefore

is the nonlinear function of δ Q.

Can get:

$\mathrm{\&Delta;}{C}_b^n=Y^T\left({\hat{Q}}_b^n\right)U\left({\hat{Q}}_b^n\right)-Y^T\left(Q_b^n\right)U\left(Q_b^n\right)=Y^T\left({\hat{Q}}_b^n\right)U\left({\hat{Q}}_b^n\right)-Y^T({\hat{Q}}_b^n-\mathrm{\&delta;Q})U({\hat{Q}}_b^n-\mathrm{\&delta;Q})$

$=Y^T\left(\mathrm{\&delta;Q}\right)U\left({\hat{Q}}_b^n\right)+Y^T\left({\hat{Q}}_b^n\right)U\left(\mathrm{\&delta;Q}\right)-Y^T\left(\mathrm{\&delta;Q}\right)U\left(\mathrm{\&delta;Q}\right)$

By $Y^T\left(\mathrm{\&delta;\; Q}\right)U\left({\hat{Q}}_b^n\right)=Y^T\left({\hat{Q}}_b^n\right)U\left(\mathrm{\&delta;\; Q}\right),$ Therefore:

$\mathrm{\&Delta;}{C}_b^n=2Y^T\left(\mathrm{\&delta;Q}\right)U\left({\hat{Q}}_b^n\right)-Y^T\left(\mathrm{\&delta;Q}\right)U\left(\mathrm{\&delta;Q}\right)=2Y^T\left(\mathrm{\&delta;Q}\right)U\left(Q_b^n\right)+Y^T\left(\mathrm{\&delta;Q}\right)U\left(\mathrm{\&delta;Q}\right)$

When attitude error was big, formula (31) was non-linear about δ Q, and nonlinear terms do

$Y^T\left(\mathrm{\&delta;Q}\right)U\left(\mathrm{\&delta;Q}\right)=\left[\begin{array}{ccc}\mathrm{\&delta;}{q}_0^2+\mathrm{\&delta;}{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000117862560000021.png,HDA0000117862560000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-\mathrm{\&delta;}{q}_2^2-\mathrm{\&delta;}{q}_3^2& 2(\mathrm{\&delta;}{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000117862560000021.png,HDA0000117862560000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_1\mathrm{\&delta;}{q}_2-\mathrm{\&delta;}{q}_0\mathrm{\&delta;}{q}_3)& 2(\mathrm{\&delta;}{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000117862560000021.png,HDA0000117862560000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_1\mathrm{\&delta;}{q}_3+\mathrm{\&delta;}{q}_0\mathrm{\&delta;}{q}_2)\\ 2(\mathrm{\&delta;}{q}_0\mathrm{\&delta;}{q}_3+\mathrm{\&delta;}{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000117862560000021.png,HDA0000117862560000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_1\mathrm{\&delta;}{q}_2)& \mathrm{\&delta;}{q}_0^2-\mathrm{\&delta;}{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000117862560000021.png,HDA0000117862560000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2+\mathrm{\&delta;}{q}_2^2\mathrm{\&delta;}{q}_3^2& 2(\mathrm{\&delta;}{q}_2\mathrm{\&delta;}{q}_3-\mathrm{\&delta;}{q}_0\mathrm{\&delta;}{q}_1)\\ 2(\mathrm{\&delta;}{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000117862560000021.png,HDA0000117862560000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_1\mathrm{\&delta;}{q}_3-\mathrm{\&delta;}{q}_0\mathrm{\&delta;}{q}_2)& 2(\mathrm{\&delta;}{q}_0\mathrm{\&delta;}{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000117862560000021.png,HDA0000117862560000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_1+\mathrm{\&delta;}{q}_2\mathrm{\&delta;}{q}_3)& \mathrm{\&delta;}{q}_0^2-\mathrm{\&delta;}{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000117862560000021.png,HDA0000117862560000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-\mathrm{\&delta;}{q}_2^2+\mathrm{\&delta;}{q}_3^2\end{array}\right]$

Model simplification:

Because the time of initial alignment is very short, so can ignore the influence of site error, accelerometer and gyrostatic measuring error are by often being worth ε at random ^b, ▽ ^bAnd white noise

Form order

Can get according to the attitude error equation:

$\mathrm{\&delta;}{\stackrel{\&CenterDot;}{Q}}_b^n=\frac{1}{2}<{\mathrm{\&omega;}}_{\mathrm{ib}}^b>\mathrm{\&delta;}{Q}_b^n-\frac{1}{2}\left[{\mathrm{\&omega;}}_{\mathrm{in}}^n\right]\mathrm{\&delta;}{Q}_b^n+\frac{1}{2}U\left({\hat{Q}}_b^n\right)({\mathrm{\&epsiv;}}^b+{\mathrm{\&omega;}}_g^b)-\frac{1}{2}Y\left({\hat{Q}}_b^n\right)\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{en}}^n$

Can get the velocity error equation is:

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{V}=-2\left[{\hat{C}}_b^n{\hat{f}}^b\right]\&times;Y^T\left(\hat{Q}\right)\mathrm{\&delta;Q}+2{\hat{C}}_b^n{\hat{f}}^b{\hat{Q}}^T\mathrm{\&delta;Q}$

$-Y^T\left(\mathrm{\&delta;q}\right)U\left(\mathrm{\&delta;Q}\right){\hat{f}}^b+C_b^n({\&dtri;}^b+{\mathrm{\&omega;}}_a^b)-({\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{in}}^n)\&times;\mathrm{\&delta;V}$

$-\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{en}}^n\&times;V^n$

Can find out; Following formula is a nonlinear equation about hypercomplex number error delta Q, is linear about noise item

still.

Set up nonlinear model:

State variable is made up of velocity error, hypercomplex number error and accelerometer and gyrostatic normal at random value, totally 13 ties up, that is:

$x=\left[\mathrm{\&delta;}{V}_E\mathrm{\&delta;}{V}_N\mathrm{\&delta;}{V}_U\mathrm{\&delta;}{q}_0\mathrm{\&delta;}{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000117862560000021.png,HDA0000117862560000022.png"\; state="\{\{state\}\}">q</figure-callout>}}_1\mathrm{\&delta;}{q}_2\mathrm{\&delta;}{q}_3{\&dtri;}_x^b{\&dtri;}_y^b{\&dtri;}_z^b{\mathrm{\&epsiv;}}_x^b{\mathrm{\&epsiv;}}_y^b{\mathrm{\&epsiv;}}_z^b\right]$

Velocity error is a measurement signal, so system equation is following:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}=f\left(x\right)+g\left(x\right)\mathrm{\&omega;}\\ z=\mathrm{Hx}+\mathrm{\&upsi;}\end{array}\right.$

Wherein

H=[I _{3 * 3}0 _{3 * 10}],

F (x) can be made up of linear and nonlinear parts, i.e. f (x)=F ₀X+p (x), wherein

$F_0=\left[\begin{array}{cccc}{F}_{0\left(11\right)}& F_{0\left(12\right)}& C_b^n& 0\\ F_{0\left(21\right)}& F_{0\left(22\right)}& 0& \frac{1}{2}U\left({\hat{Q}}_b^n\right)\\ 0& 0& 0& 0\end{array}\right]$

In the formula

$F_{0\left(21\right)}=-\frac{1}{2}\left[\begin{array}{ccc}-{\hat{q}}_1& -{\hat{q}}_2& -{\hat{q}}_3\\ {\hat{q}}_0& {\hat{q}}_3& -{\hat{q}}_2\\ -{\hat{q}}_3& {\hat{q}}_0& {\hat{q}}_1\\ {\hat{q}}_2& -{\hat{q}}_1& {\hat{q}}_0\end{array}\right]\left[\begin{array}{ccc}0& -1/(R_M+h)& 0\\ 1/R_N+h& 0& 0\\ \tan L/(R_N+h)& 0& 0\end{array}\right],$

$F_{0\left(12\right)}=-2\left[{\hat{C}}_b^n{\hat{f}}^b\right]\&times;Y^T\left(\hat{Q}\right)+2{\hat{C}}_b^n{\hat{f}}^b{\hat{Q}}^T,$ $F_{0\left(22\right)}=\frac{1}{2}<{\mathrm{\&omega;}}_{\mathrm{ib}}^b>-\frac{1}{2}\left[{\mathrm{\&omega;}}_{\mathrm{in}}^n\right],$

Non-linear partial $p\left(x\right)=\left[\begin{array}{c}{Y}^T\left(\mathrm{\&delta;\; Q}\right)U\left(\mathrm{\&delta;\; Q}\right){\hat{f}}^b\\ 0\end{array}\right].$

(4) the improved UKF filtering algorithm described in the present invention is following:

Complicated noise model is following:

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+g\left(x_{k-1}\right){\mathrm{\&omega;}}_{k-1}\\ z_k=h\left(x_k\right)+k\left(x_k\right)v_k\end{array}\right.$

Wherein, f (), g (), h (), k () is nonlinear function, x _k, z _kBe respectively state and measurement vector, ω _k, v _kBe respectively state-noise and measurement noise vector, and

Following formula is linear about noise.

Init state variable and mean square deviation

${\hat{x}}_0=e\left(x_0\right),$ $P_0=E\left[({\hat{x}}_0-x_0){({\hat{x}}_0-x_0)}^T\right]$

Time upgrades

$\left\{\begin{array}{c}{\mathrm{\&chi;}}_{k-1}=[{\hat{x}}_{k-1}{\left[{\hat{x}}_{k-1}\right]}_L\&PlusMinus;\mathrm{\&gamma;}\sqrt{P_{k-1}}\\ {\mathrm{\&chi;}}_{i,k|k-1}^{*}=f\left({\mathrm{\&chi;}}_{i,k-1}\right)\\ {\hat{x}}_{k|k-1}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^{\left(m\right)}{\mathrm{\&chi;}}_{i,k|k-1}^{*}\\ P_{k|k-1}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}({\mathrm{\&chi;}}_{i,k|k-1}^{*}-{\hat{x}}_{k\backslash k-1})\\ {\mathrm{\&chi;}}_{k|k-1}=[{\hat{x}}_{k|k-1}{\left[{\hat{x}}_{k|k-1}\right]}_L\&PlusMinus;\mathrm{\&gamma;}\sqrt{P_{k|k-1}}\\ {\mathrm{\&eta;}}_{i,k|k-1}=h\left({\mathrm{\&chi;}}_{i,k|k-1}\right)\\ {\hat{z}}_{k|k-1}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^{\left(m\right)}{\mathrm{\&eta;}}_{i,k|k-1}\end{array},{({\mathrm{\&chi;}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1})}^T+g\left({\hat{x}}_{k-1}\right)Q_{k-1}g{\left({\hat{x}}_{k-1}\right)}^T\right.$

Measure and upgrade

$\left\{\begin{array}{c}{P}_{{\hat{z}}_k{\hat{z}}_k}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}({\mathrm{\&eta;}}_{i,k|k-1}-{\hat{z}}_{k|k-1}){({\mathrm{\&eta;}}_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T+k\left({\hat{x}}_{k|k-1}\right)R_{k}k{\left({\hat{x}}_{k|k-1}\right)}^T\\ P_{{\hat{x}}_k{\hat{z}}_k}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_{k|k-1}){({\mathrm{\&eta;}}_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T+k\left({\hat{x}}_{k|k-1}\right)R_{k}k{\left({\hat{x}}_{k|k-1}\right)}^T\\ K_k=P_{{\hat{x}}_k{\hat{z}}_k}{P}_{{\hat{z}}_k{\hat{z}}_k}^{-1}\\ {\hat{x}}_k={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})\\ P_k=P_{k|k-1}-K_k{P}_{{\hat{z}}_k{\hat{z}}_k}{K}_k^T\end{array}\right.$

Wherein, Represent a matrix, all column vectors are all by L x _kForm, all the other parameters are following:

$\left\{\begin{array}{c}\mathrm{\&lambda;}={\mathrm{\&alpha;}}^2(L+\mathrm{\&kappa;})-L\\ \mathrm{\&gamma;}=\sqrt{L+\mathrm{\&lambda;}}\\ W_0^{\left(m\right)}=\mathrm{\&lambda;}/(L+\mathrm{\&lambda;}),W_0^{\left(c\right)}=\mathrm{\&lambda;}/(L+\mathrm{\&lambda;})+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})\\ W_i^{\left(m\right)}=W_i^{\left(c\right)}=1/\left[2(L+\mathrm{\&lambda;})\right],i(=\mathrm{1,2},...,2L)\end{array}\right.$

L is x _kDimension; α is used for confirming near the distribution situation (generally speaking 1e-4≤α≤1) of Sigma point average; κ is that a scale factor (is made as 0 when state estimation; Be 3-L when parameter estimation), β is another scale factor (being used for the priori that merging phase distributes, is 2 for the Gaussian distribution optimal value).

If model is linear, can be reduced to:

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+g\left(x_{k-1}\right){\mathrm{\&omega;}}_{k-1}\\ z_k=H_k{x}_k+v_k\end{array}\right.$

Then can further simplify Sigma sampled point χ again to above algorithm _K|k-1Satisfy equation:

$\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_{k|k-1}){({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_{k|k-1})}^T=P_{k|k-1}$

Can get η according to measurement equation _{I, k|k-1}=H _kχ _{I, k|k-1}With So

$\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}({\mathrm{\&eta;}}_{i,k|k-1}-{\hat{x}}_{k|k-1}){({\mathrm{\&eta;}}_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T$

$=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}\left[H_k({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_{k|k-1})\right]{\left[H_k({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_{k|k-1})\right]}^T$

$=H_k{P}_{k|k-1}{H}_k^T$

Therefore:

$P_{{\hat{z}}_k{\hat{z}}_k}=H_k{P}_{k|k-1}{H}_k^T+R_k$

In like manner $P_{{\hat{x}}_k{\hat{z}}_k}=P_{k|k-1}{H}_k^T$

Can know that by above derivation the UKF filtering algorithm of simplification is when the measurement equation is linear equation:

$\left\{\begin{array}{c}{\mathrm{\&chi;}}_{k-1}=[{\hat{x}}_{k-1}{\left[x_k\right]}_L\&PlusMinus;\mathrm{\&gamma;}\sqrt{P_{k-1}}]\\ {\mathrm{\&chi;}}_{i,k|k-1}^{*}=f\left({\mathrm{\&chi;}}_{i,k-1}\right)\\ {\hat{x}}_{k|k-1}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^{\left(m\right)}{\mathrm{\&chi;}}_{i,k|k-1}^{*}\\ P_{k|k-1}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}({\mathrm{\&chi;}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1}){({\mathrm{\&chi;}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1})}^T+g\left({\hat{x}}_{k-1}\right)Q_{k-1}g{\left({\hat{x}}_{k-1}\right)}^T\\ P_{{\hat{x}}_k{\hat{z}}_k}=P_{k|k-1}{H}_k^T,P_{{\hat{z}}_k{\hat{z}}_k}=H_k{P}_{k|k-1}{H}_k^T+R_k\\ K_k=P_{{\hat{x}}_k{\hat{z}}_k}{P}_{{\hat{z}}_k{\hat{z}}_k}^{-1}\\ {\hat{z}}_{k|k-1}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{H}_k{\hat{x}}_{k|k-1}\\ {\hat{x}}_k={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})\\ P_k=P_{k|k-1}-K_k{P}_{{\hat{z}}_k{\hat{z}}_k}{K}_k^T\end{array}\right.$

The present invention adopt a kind of fast, high precision, passive strapdown inertial navigation system non-linear alignment method, below this beneficial effect of the invention of simulating, verifying.

In order to verify the validity of this method, under big misalignment and little misalignment situation, do contrast simulation with EKF filtering and conventional kalman.

Simulated conditions: gyroscope constant value drift is 0.05 °/h, 0.05 °/h of stochastic error; Jia Biaochang value biasing 50 μ g, stochastic error 50 μ g; Latitude 32 degree, longitude 118 degree; The posture renewal cycle is 10ms, and the filtering cycle is 1s, population N=1000.

Under the quiet pedestal condition strap down inertial navigation system is being carried out emulation respectively, simulation time 600s under big misalignment and two kinds of situation of little misalignment.

(1) emulation under the little misalignment situation

Simulation result is as shown in Figure 7 when misalignment being set being low-angle [0.1 ° 0.1 ° 1 °].

Fig. 7 (a)-(c) is respectively pitch angle, roll angle and the course angle phantom error curve of simplifying UKF, conventional kalman and EKF filtering; The alignment precision of three kinds of methods is suitable, and horizontal attitude is aligned in and has just reached stable state in very short time, and the alignment of orientation steady-state value is all at 0.1 degree; But the stabilization time of the conventional kalman in alignment speed aspect is the shortest; UKF is suitable with EKF speed, and the about 90s of UKF filtering reaches stationary value, and the about 110s of EKF filtering reaches steady-state value.

(2) emulation under the big misalignment situation

Simulation result is as shown in Figure 8 when misalignment being set being wide-angle [10 ° 10 ° 30 °].

Fig. 8 (a)-(c) is respectively pitch angle, roll angle and the course angle phantom error curve of simplifying UKF, conventional kalman and EKF filtering; Three kinds of methods have demonstrated more significantly difference under big misalignment situation; Then can not accomplish aligning with conventional kalman Filtering Model, three attitude angle are shakes significantly and can not restrain; UKF and EKF still can accomplish aligning in the short period of time, and the horizontal attitude alignment precision is suitable, but alignment of orientation precision UKF will be higher than EKF; The UKF stable state accuracy is at 0.1 degree; EKF is at 0.2 degree, and this is because the EKF high-order truncation error that linearization brings to system equation has reduced filtering accuracy, but UKF is because its algorithm is independent of nonlinear model; Its filtering accuracy is constant, and it is short to aim at time ratio EKF.

Claims (3)

1. strapdown inertial navigation system non-linear alignment method is characterized in that comprising step:

(1) under quiet pedestal condition, gathers the data of fiber strapdown inertial navigation system accelerometer and fibre optic gyroscope, and do denoising;

(2) utilize analytical method tentatively to accomplish the coarse alignment process of system, obtain the approximate down attitude matrix of day coordinate system ENU northeastward

Wherein

θ ₀And γ ₀Be respectively angle, initial heading, initial pitch angle and initial horizontal cradle angle;

(3) use the fine alignment process that the compass method is accomplished strapdown inertial navigation system, comprise horizontal fine alignment and orientation fine alignment, obtain attitude matrix

Corresponding course angle

Pitch angle θ ₁With roll angle γ ₁

(4) set up the strapdown inertial navigation system nonlinearity erron model based on hypercomplex number that is big misalignment to attitude angle and position angle, foundation is with the observation model of speed as observed quantity;

Described foundation is following to the strapdown inertial navigation system nonlinearity erron model based on hypercomplex number that attitude angle and position angle are big misalignment:

Attitude error equations:

$\mathrm{\&delta;}{\stackrel{\&CenterDot;}{Q}}_b^n=\frac{1}{2}<{\mathrm{\&omega;}}_{\mathrm{ib}}^b>\mathrm{\&delta;}{Q}_b^n-\frac{1}{2}\left[{\mathrm{\&omega;}}_{\mathrm{in}}^n\right]\mathrm{\&delta;}{Q}_b^n+\frac{1}{2}U\left({\hat{Q}}_b^n\right)({\mathrm{\&epsiv;}}^b+{\mathrm{\&omega;}}_g^b)-\frac{1}{2}Y\left({\hat{Q}}_b^n\right)\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{en}}^n,$

Be the linear differential equation of hypercomplex number, in the formula,

Be the error of hypercomplex number, $<{\mathrm{\&omega;}}_{\mathrm{Ib}}^b>=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_x& -{\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_z\\ {\mathrm{\&omega;}}_x& 0& {\mathrm{\&omega;}}_z& -{\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_z& 0& {\mathrm{\&omega;}}_x\\ {\mathrm{\&omega;}}_z& {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_x& 0\end{array}\right],$

ω wherein _x, ω _y, ω _zBe respectively following three the direction gyro angular speeds of carrier coordinate system, $\left[{\mathrm{\&omega;}}_{\mathrm{In}}^n\right]=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_E& -{\mathrm{\&omega;}}_N& -{\mathrm{\&omega;}}_U\\ {\mathrm{\&omega;}}_E& 0& -{\mathrm{\&omega;}}_U& {\mathrm{\&omega;}}_N\\ {\mathrm{\&omega;}}_N& {\mathrm{\&omega;}}_U& 0& -{\mathrm{\&omega;}}_E\\ {\mathrm{\&omega;}}_U& -{\mathrm{\&omega;}}_N& {\mathrm{\&omega;}}_E& 0\end{array}\right],$

ω wherein _E, ω _N, ω _UBe respectively navigation coordinate system (ENU) following three direction gyro angular speeds,

$U\left({\hat{Q}}_b^n\right)\stackrel{\mathrm{\&Delta;}}{=}U=\left[\begin{array}{ccc}-{\hat{q}}_1& -{\hat{q}}_2& -{\hat{q}}_3\\ {\hat{q}}_0& {\hat{q}}_3& {\hat{q}}_2\\ {\hat{q}}_3& {\hat{q}}_0& -{\hat{q}}_1\\ -{\hat{q}}_2& {\hat{q}}_1& {\hat{q}}_0\end{array}\right],$ $Y\left({\hat{Q}}_b^n\right)\stackrel{\mathrm{\&Delta;}}{=}Y=\left[\begin{array}{ccc}-{\hat{q}}_1& -{\hat{q}}_2& -{\hat{q}}_3\\ {\hat{q}}_0& {\hat{q}}_3& -{\hat{q}}_2\\ -{\hat{q}}_3& {\hat{q}}_0& {\hat{q}}_1\\ {\hat{q}}_2& -{\hat{q}}_1& {\hat{q}}_0\end{array}\right],$

[q wherein ₀, q ₁, q ₂, q ₃] be hypercomplex number,

Be the projection of gyro angular velocity under carrier coordinate system, ε ^bBe gyroscopic drift,

Be corresponding gyro to measure white noise,

The expression navigation coordinate is the projection of angular velocity under navigation system of spherical coordinate system relatively, in whole derivation, does not suppose that misalignment is that therefore, this equation can be described carrier and be the attitude error propagation characteristic under the big misalignment three attitudes in a small amount;

The velocity error equation:

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{V}=-2\left[{\hat{C}}_b^n{\hat{f}}^b\right]\&times;Y^T\left(\hat{Q}\right)\mathrm{\&delta;Q}+2{\hat{C}}_b^n{\hat{f}}^b{\hat{Q}}^T\mathrm{\&delta;Q}-Y^T\left(\mathrm{\&delta;Q}\right)U\left(\mathrm{\&delta;Q}\right){\hat{f}}^b+C_b^n({\&dtri;}^b+{\mathrm{\&omega;}}_a^b)-({\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{in}}^n)\&times;\mathrm{\&delta;V}-\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{en}}^n\&times;V^n,$

In the formula,

Be the calculated value of attitude battle array,

Be acceleration measuring value, ▽ ^bBe the accelerometer biasing,

Be the accelerometer measures white noise, δ g ⁿBe the projection of gravity acceleration error under navigation coordinate system;

The site error equation:

$\left[\begin{array}{c}\mathrm{\&delta;}\stackrel{\&CenterDot;}{L}\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{h}\end{array}\right]=\left[\begin{array}{ccc}0& 0& -\stackrel{\&CenterDot;}{L}/(R_M+h)\\ \stackrel{\&CenterDot;}{\mathrm{\&lambda;}}\tan \mathrm{\&lambda;}& 0& -\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}/(R_N+h)\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;L}\\ \mathrm{\&delta;\&lambda;}\\ \mathrm{\&delta;h}\end{array}\right]+\left[\begin{array}{ccc}0& 1/(R_M+h)& 0\\ \sec L/(R_N+h)& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;}{v}_E\\ \mathrm{\&delta;}{v}_N\\ \mathrm{\&delta;}{v}_U\end{array}\right],$

In the formula, δ L, δ λ, δ h is respectively latitude error, longitude error and height error, R _MBe the radius-of-curvature in the reference ellipsoid meridian ellipse, R _M=R _e(1-2e+3e sin ²L), R _NBe the radius-of-curvature in the plane normal of vertical meridian ellipse, R _N=R _e(1+esin ²L), R wherein _eBe the major axis radius of reference ellipsoid, e is the ovality of ellipsoid;

State vector is got $x=\left[\mathrm{\&delta;}{V}_E\mathrm{\&delta;}{V}_N\mathrm{\&delta;}{V}_U\mathrm{\&delta;}{q}_0\mathrm{\&delta;}{q}_1\mathrm{\&delta;}{q}_2\mathrm{\&delta;}{q}_3\mathrm{\&delta;\; L\&delta;\; \&lambda;\; \&delta;\; h}{\&dtri;}_x^b{\&dtri;}_y^b{\&dtri;}_z^b{\mathrm{\&epsiv;}}_x^b{\mathrm{\&epsiv;}}_y^b{\mathrm{\&epsiv;}}_z^b\right],$ Noise vector is got

Set up the filter state model, and with velocity error Z=δ v=[δ v _x, δ v _y] ^TFor observation equation is set up in observed quantity: $\left\{\begin{array}{c}\stackrel{\&CenterDot;}{X}=f\left(X\right)+g\left(X\right)W\\ Z=\mathrm{HX}+V\end{array}\right.,$ H=[0 _{3 * 4}, I _{3 * 3}, 0 _{3 * 6}], V is for measuring noise;

(5) the initial alignment nonlinear model according to step (4) improves nonlinear filtering algorithm UKF; With improved UKF the model of step (4) is carried out the iteration Filtering Estimation; Obtain the platform misalignment; Constantly the strap down inertial navigation system attitude matrix in last cycle of close-loop feedback correction obtains attitude matrix thereby accomplish accurate initial alignment

Described when the measurement equation is linear equation, improved UKF filtering method is following:

$\left\{\begin{array}{c}{\mathrm{\&chi;}}_{k-1}=[{\hat{x}}_{k-1}{\left[x_k\right]}_L\&PlusMinus;\mathrm{\&gamma;}\sqrt{P_{k-1}}]\\ {\mathrm{\&chi;}}_{i,k|k-1}^{*}=f\left({\mathrm{\&chi;}}_{i,k-1}\right)\\ {\hat{x}}_{k|k-1}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^{\left(m\right)}{\mathrm{\&chi;}}_{i,k|k-1}^{*}\\ P_{k|k-1}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}({\mathrm{\&chi;}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1}){({\mathrm{\&chi;}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1})}^T+g\left({\hat{x}}_{k-1}\right)Q_{k-1}g{\left({\hat{x}}_{k-1}\right)}^T\\ P_{{\hat{x}}_k{\hat{z}}_k}=P_{k|k-1}{H}_k^T,P_{{\hat{z}}_k{\hat{z}}_k}=H_k{P}_{k|k-1}{H}_k^T+R_k\\ K_k=P_{{\hat{x}}_k{\hat{z}}_k}{P}_{{\hat{x}}_k{\hat{z}}_k}^{-1}\\ {\hat{z}}_{k|k-1}=\underset{i=0}{\overset{2L}{\mathrm{\&Sigma;}}}{H}_k{\hat{x}}_{k|k-1}\\ {\hat{x}}_k={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})\\ P_k=P_{k|k-1}-K_k{P}_{{\hat{z}}_k{\hat{z}}_k}{K}_k^T\end{array}\right.$

In the formula, χ _K-1Be k-1 sigma sampled point constantly,

Be k-1 state estimation constantly,

Represent a matrix, all column vectors are all by L x _kForm P _kBe the state variance matrix,

One-step prediction sigma sampled point,

Be state one-step prediction, K _kBe gain matrix,

For measuring the one-step prediction average, all the other calculation of parameter are following:

$\left\{\begin{array}{c}\mathrm{\&lambda;}={\mathrm{\&alpha;}}^2(L+\mathrm{\&kappa;})-L\\ \mathrm{\&gamma;}=\sqrt{L+\mathrm{\&lambda;}}\\ W_0^{\left(m\right)}=\mathrm{\&lambda;}/(L+\mathrm{\&lambda;}),W_0^{\left(c\right)}=\mathrm{\&lambda;}/(L+\mathrm{\&lambda;})+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})\\ W_i^{\left(m\right)}=W_i^{\left(c\right)}=1/\left[2(L+\mathrm{\&lambda;})\right],i(=\mathrm{1,2},...,2L)\end{array}\right.;$

Utilize above-mentioned improved UKF filtering algorithm to obtain hypercomplex number variable quantity δ Q, convert platform misalignment matrix into Thereby constantly revise the initial matrix that step (3) obtains

Obtain the final initial alignment attitude matrix that satisfies precision $C_{b\left(2\right)}^n={\left(C_n^{n^{\&prime;}}\right)}^T{C}_{b\left(1\right)}^n.$

2. a kind of strapdown inertial navigation system non-linear alignment method according to claim 1; It is characterized in that: in the step (1); Accelerometer and optical fibre gyro data are carried out the denoising method; Adopt wavelet multiresolution rate analytical technology to carry out denoising, remove and be included in noise and the undesired signal in the high-frequency signal.

3. a kind of strapdown inertial navigation system non-linear alignment method according to claim 1 is characterized in that:

In the step (2), analytical method coarse alignment method adopts any of following two kinds of methods:

(a) utilize the vectorial g of three mutually orthogonals, g * ω _Ie(g * ω _Ie) * g calculates

Elder generation is with the vectorial a of three mutually orthogonals ^b, a ^b* ω ^b(a ^b* ω ^b) * a ^bCalculate

$C_{b\left(0\right)}^{\&prime;n}={\left[\begin{array}{c}{\left(g^n\right)}^T\\ {(g^n\&times;{\mathrm{\&omega;}}_{\mathrm{ie}}^n)}^T\\ {[(g^n\&times;{\mathrm{\&omega;}}_{\mathrm{ie}}^n)\&times;g^n]}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left(a^b\right)}^T\\ {(a^b\&times;{\mathrm{\&omega;}}^b)}^T\\ {[(a^b\&times;{\mathrm{\&omega;}}^b)\&times;a^b]}^T\end{array}\right]$

Wherein, g ⁿThe projection of expression acceleration of gravity under navigation coordinate system,

The projection of expression rotational-angular velocity of the earth under navigation coordinate system, a ^bExpression acceleration measuring value, ω ^bExpression gyroscope survey value, a ^b=g ^b+ δ a ^b,

δ a ^bThe measuring error of expression accelerometer, δ ω ^bRepresent gyrostatic measuring error;

Again attitude matrix

being carried out normalization handles:

$C_{b\left(0\right)}^n=C_{b\left(0\right)}^{\&prime;n}{\left[{\left(C_{b\left(0\right)}^{\&prime;n}\right)}^T{C}_{b\left(0\right)}^{\&prime;n}\right]}^{-\frac{1}{2}},$

is exactly the attitude matrix that coarse alignment obtains;

(b) directly calculate

by measured value

First the navigation frame transfer to the carrier matrix of the coordinate system

wrote:

C wherein ₁, C ₂, C ₃For constituting

Three column vectors, and have quadrature constraint to each other;

If the measured value of k moment gyroscope and accelerometer is designated as ω respectively ^b(k) and a ^b(k), adopt the least square method under the quadrature constraint to estimate

Three branch column vectors:

${\hat{C}}_3=-\frac{1}{\mathrm{\&lambda;}}\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{a}^b\left(k\right)$

${\hat{C}}_2=\frac{1}{\mathrm{\&eta;}}(\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}^b\left(k\right)+\mathrm{\&rho;}\hat{z})$

${\hat{C}}_1=\hat{C}2\&times;\hat{C}3=\frac{1}{\mathrm{\&eta;}}\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}^b\left(k\right)\&times;{\hat{C}}_3$

Wherein, N is total measurement number of times,

$\mathrm{\&lambda;}={\left[{\left(\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{a}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{a}^b\left(k\right)\right)\right]}^{\frac{1}{2}}$

$\mathrm{\&rho;}=\frac{1}{\mathrm{\&lambda;}}{\left(\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{a}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}^b\left(k\right)\right)$

$\mathrm{\&eta;}={[{\left(\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}^b\left(k\right)\right)}^T\left(\underset{k=1}{\overset{N}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}^b\left(k\right)\right)-{\mathrm{\&rho;}}^2]}^{\frac{1}{2}}$

Then the estimated value of initial attitude matrix is:

that calculate has been orthogonal matrix, do not need to do orthogonalization process again.

Images