CN102620748A - Method for estimating and compensating lever arm effect in case of shaken base by strapdown inertial navigation system - Google Patents

Abstract

The invention provides a method for estimating and compensating a lever arm effect in the case of a shaken base by a strapdown inertial navigation system, which is used for estimating the influence of the lever arm effect to the system and compensating according to a certain strategy, so that the precision of fine alignment and navigation calculation can be improved. The method comprises the following steps of: firstly, on the basis of coarse alignment, dividing a fine alignment process into two stages, wherein at a first stage: running a parameter identification method fine alignment algorithm of lever arm speed real-time compensating and spreading variable quantity, to estimate the residual interference speed except the lever arm speed in the system, and at a second stage: carrying out primary compensation on the residual interference speed; continuously running the parameter identification method fine alignment algorithm of the lever arm speed real-time compensating and spreading variable quantity, to estimate the misalignment angle information; and further modifying a coarse alignment result by the estimated misalignment angle, and confirming an initial pose matrix, so that the fine alignment can be completed. In the stage of navigation calculation, the lever arm speed is compensated in real time, a strapdown calculating program is operated, and a navigation result is provided.

Description

SINS rocks the estimation and the compensation method of lever arm effect under the pedestal condition

Technical field

The invention belongs to the inertial navigation technical field, relate to inertial navigation system, is that a kind of SINS rocks under the pedestal condition, the estimation and the compensation method of the velocity error that is caused by lever arm effect in fine alignment and navigation stage.

Background technology

Inertial technology is one of technical field of each industrial powers development of the world; Inertial navigation system (INS; Be called for short inertial navigation) be a kind of system that utilizes inertial technology to realize the carrier independent navigation, inertial navigation does not have the entity platform, and the interference of rocking of carrier directly adds to gyroscope and accelerometer.Initial alignment is one of SINS gordian technique, and it provides the initial value that resolves for navigational system, and accuracy of navigation systems is had very big influence, becomes the focus of Chinese scholars research in recent years.External interference can make sensor output signal introduce noise, promptly influences the reliability of sensor signal, and then influence is aimed at and navigation accuracy.

Lever arm effect is because under inertial measurement cluster installation site and situation that the carrier swing center does not overlap; Carrier receives external interference or carrier movement and the SINS pedestal is in wave or vibrating state; Cause accelerometer output information to be interfered, and then influence velocity information.The interference that lever arm effect causes can cause the error that navigational system initial alignment and navigation calculation are very big, must estimate and compensate.

Because the factors such as the difficult estimation of lever arm velocity error that the angular speed of inaccurate, the carrier movement of lever arm length computation and angle rate of acceleration estimating noise are big, initial, traditional lever arm speed dynamics compensation method can not full remuneration lever arm error, has remnants.Cause high frequency interference through low pass filtering method filtering lever arm effect, can cause useful information to be lost, sensor information is asynchronous, degradation problem under the alignment precision.Design and a kind ofly rock that lever arm effect new under the pedestal condition is estimated and the method for compensation has the important engineering practical value.

Summary of the invention

The problem that the present invention will solve is: lever arm effect has very big influence to the initial alignment and the navigation calculation of SINS, and existing technological means can not effectively compensate lever arm effect.The present invention mainly solves and rocks under the pedestal condition, how to estimate lever arm effect and the problem that how to compensate its influence at aligning and navigation stage.

Technical scheme of the present invention is: a kind of SINS rocks the estimation and the compensation method of lever arm effect under the pedestal condition, estimates that lever arm effect to the influence of SINS and compensate, may further comprise the steps:

1) SINS start preheating, the output data of gathering inertial measurement cluster;

2) carry out coarse alignment, obtain rough initial attitude matrix

3) on the coarse alignment basis, divide two stages to accomplish the fine alignment process;

31) set up the parameter identification method fine alignment mathematical model of expansion variable, tectonic system equation and observation equation; The parameter identification method fine alignment mathematical model of expansion variable:

$\left\{\begin{array}{c}\mathrm{\&Delta;}{V}_e=({\&dtri;}_e-g.{\mathrm{\&phi;}}_{n0})t-\frac{{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="HDA0000145898280000022.png,HDA0000145898280000031.png"\; state="\{\{state\}\}">t</figure-callout>}}^2}{2}g{u}_n+\frac{{\mathrm{<figure-callout\; id="3"\; label="t"\; filenames="HDA0000145898280000022.png,HDA0000145898280000031.png"\; state="\{\{state\}\}">t</figure-callout>}}^3}{6}g{\mathrm{\&omega;}}_{\mathrm{ie}}{u}_e\sin L+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\&Delta;}{V}_n=({\&dtri;}_n+g.{\mathrm{\&phi;}}_{e0})t+\frac{{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="HDA0000145898280000022.png,HDA0000145898280000031.png"\; state="\{\{state\}\}">t</figure-callout>}}^2}{2}g{u}_e+\frac{{\mathrm{<figure-callout\; id="3"\; label="t"\; filenames="HDA0000145898280000022.png,HDA0000145898280000031.png"\; state="\{\{state\}\}">t</figure-callout>}}^3}{6}g{\mathrm{\&omega;}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)+V_{\mathrm{dn}}++V_{\mathrm{sn}}\end{array}\right.$

Wherein $\left\{\begin{array}{c}{u}_e={\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">n</figure-callout>}0}{\mathrm{\&omega;}}_{\mathrm{Ie}}\mathrm{Sin}L-{\mathrm{\&phi;}}_{u0}{\mathrm{\&omega;}}_{\mathrm{Ie}}\mathrm{Cos}L-{\mathrm{\&epsiv;}}_e\\ u_n=-{\mathrm{\&phi;}}_{e0}{\mathrm{\&omega;}}_{\mathrm{Ie}}\mathrm{Sin}L-{\mathrm{\&epsiv;}}_n\\ u_u={\mathrm{\&phi;}}_{e0}{\mathrm{\&omega;}}_{\mathrm{Ie}}\mathrm{Cos}L-{\mathrm{\&epsiv;}}_u\end{array}\right.$

In the formula: Represent the normal value biasing of accelerometer equivalence east orientation and north orientation respectively; φ _E0, φ _N0, φ _U0Represent initial misalignment; ω _e, ω _n, ω _uExpression equivalence east, north, sky are to gyroscope constant value drift; ω _IeExpression earth rotation angular speed; L representes local latitude; G representes the terrestrial gravitation acceleration; V _Se, V _SnRepresent east orientation and north orientation random disturbance speed respectively, V _De, V _DnExpression east orientation and north orientation residual interference speed are normal value; Δ V _e, Δ V _nRepresent east orientation and north orientation velocity error respectively; Be that the speed resolved of SINS is rejected the poor of velocity amplitude that value and outside reference after the lever arm speed provide; Rocking under the pedestal condition; The speed that outside reference provides is 0m/s, and the process of from the speed that SINS resolves, rejecting lever arm speed is the real-Time Compensation of lever arm speed, and the lever arm speed that is caused by lever arm effect calculates according to the lever arm rate pattern:

$\mathrm{\&delta;}{v}_g={\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;r=\left(\begin{array}{c}{\mathrm{\&omega;}}_{\mathrm{iby}}^b\&CenterDot;r_z-{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\&CenterDot;r_y\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b\&CenterDot;r_x-{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\&CenterDot;r_z\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\&CenterDot;r_y-{\mathrm{\&omega;}}_{\mathrm{iby}}^b\&CenterDot;r_x\end{array}\right)$

In the formula: δ v _gExpression lever arm speed; R=(r _x, r _y, r _z) indication rod arm lengths vector, this value is calculated in advance and is set into system according to the project organization and the navigational system installation site of carrier, and in the practical application, lever arm length vector can be because factors such as carrier deflection deformation, load distribution variation depart from this value; Represent the gyroscope output angle speed on three directions;

Said residual interference speed comprises: the random disturbance speed that system exists; The lever arm speed calculation error that, carrier inaccurate because of the lever arm linear measure longimetry exists the factors such as angular speed existence interference of deflection deformation, gyroscope survey to cause; Rocking under the pedestal condition, strapdown resolves in the speed of initial time and has lever arm speed, and when the operation strapdown resolved algorithm, initial velocity press 0m/s processing, the residual speed that causes; Behind the coarse alignment, there is error in initial attitude matrix, when the operation strapdown resolves algorithm, and the velocity error of bringing;

According to the parameter identification method mathematical model of expansion variable, tectonic system equation and observation equation are:

With SINS medium velocity error is observed quantity, is rewritten into following form to the parameter identification method mathematical model of expansion variable:

$\left\{\begin{array}{c}\mathrm{\&Delta;}{V}_e=a_{1e}\left(\mathrm{KT}\right)+a_{2e}{\left(\mathrm{KT}\right)}^2+a_{3e}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\&Delta;}{V}_n=a_{1n}\left(\mathrm{KT}\right)+a_{2n}{\left(\mathrm{KT}\right)}^2+a_{3n}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{dn}}+V_{\mathrm{sn}}\end{array}\right.,K=\mathrm{0,1,2},\&CenterDot;\&CenterDot;\&CenterDot;$

In the formula: T is the sensor data samples cycle of SINS;

$\left\{\begin{array}{c}{a}_{1e}=({\&dtri;}_e-g.{\mathrm{\&phi;}}_{n0}),a_{2e}=-\frac{1}{2}g{u}_n,a_{3e}=\frac{1}{6}g{\mathrm{\&omega;}}_{\mathrm{ie}}{u}_e\sin L\\ a_{1n}=({\&dtri;}_n+g.{\mathrm{\&phi;}}_{e0}),a_{2n}=\frac{1}{2}g{u}_e,a_{3n}=\frac{1}{6}g{\mathrm{\&omega;}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\end{array}\right.$

East orientation and north orientation velocity error Δ V _e, Δ V _eAs observed quantity, a _1e, a _2e, a _3e, V _De, a _1n, a _2n, a _3n, V _DnAs treating identified parameters, tectonic system equation and observation equation:

Identified parameters is treated in definition, and promptly system state variables is:

$X_e=\left[\begin{array}{c}{a}_{1e}\\ a_{2e}\\ a_{3e}\\ V_{\mathrm{de}}\end{array}\right],$ $X_n=\left[\begin{array}{c}{a}_{1n}\\ a_{2n}\\ a_{3n}\\ V_{\mathrm{dn}}\end{array}\right]$

List system equation and observation equation:

$\left\{\begin{array}{c}{X}_e(k+1)=X_e\left(k\right)\\ \mathrm{\&Delta;}{V}_e\left(k\right)=H\left(k\right)X_e\left(k\right)+V_{\mathrm{ge}}\left(k\right)\end{array}\right.,$ $\left\{\begin{array}{c}{X}_n(k+1)=X_n\left(k\right)\\ \mathrm{\&Delta;}{V}_n\left(k\right)=H\left(k\right)X_n\left(k\right)+V_{\mathrm{gn}}\left(k\right)\end{array}\right.$

In the formula: observing matrix H (k)=[kT, (kT) ², (kT) ³, 1]; V _Ge(k), V _Gn(k) the speed observation noise of expression east orientation and north orientation, its variance intensity is R _e, R _n

32) the fine alignment phase one; According to the coarse alignment result, the operation strapdown resolves program, carries out the real-Time Compensation of lever arm speed; Simultaneously according to step 31) system equation and the observation equation estimating system state variable set up, the 4th component of state variable is residual interference speed;

33) fine alignment subordinate phase: residual interference speed is carried out single compensation; Then, continue the lever arm speed real-Time Compensation of operation fine alignment phase one and, and carry out initial misalignment and estimate, after the initial misalignment convergence to be estimated, estimated value φ to the algorithm for estimating of system state variables _E0, φ _N0, φ _U0Calculate the misalignment φ of current time in the substitution misalignment Changing Pattern model _e, φ _n, φ _uThe misalignment information of utilizing current time is to the coarse alignment result

Carry out a step and revise, obtain the attitude matrix of current time Pass through attitude matrix

Extract position angle H, pitch angle P and roll angle R, fine alignment is accomplished;

Wherein, according to step 32) estimate that the state variable obtain calculates u _e, u _n, u _u, φ _E0, φ _N0And φ _U0:

$u_e=\frac{2a_{2n}}{g},$ $u_n=-\frac{2a_{2e}}{g},$ $u_u=-\frac{6a_{3n}}{g{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L}-\frac{2a_{2e}}{g}\tan L$

${\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="e"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">e</figure-callout>}0}=\frac{a_{1n}}{g},$ ${\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">n</figure-callout>}0}=-\frac{a_{1e}}{g},$ ${\mathrm{\&phi;}}_{u0}={\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">n</figure-callout>}0}.\tan L-\frac{u_e}{{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L}$

According to initial misalignment and misalignment Changing Pattern Model Calculation current time misalignment:

$\left\{\begin{array}{c}{\mathrm{\&phi;}}_e={\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="e"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">e</figure-callout>}0}+u_e.t+\frac{{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="HDA0000145898280000022.png,HDA0000145898280000031.png"\; state="\{\{state\}\}">t</figure-callout>}}^2}{2}.{\mathrm{\&omega;}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\\ {\mathrm{\&phi;}}_n={\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">n</figure-callout>}0}+u_n.t-\frac{{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="HDA0000145898280000022.png,HDA0000145898280000031.png"\; state="\{\{state\}\}">t</figure-callout>}}^2}{2}.{\mathrm{\&omega;}}_{\mathrm{ie}}{u}_e\sin L\\ {\mathrm{\&phi;}}_u={\mathrm{\&phi;}}_{u0}+u_u.t+\frac{{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="HDA0000145898280000022.png,HDA0000145898280000031.png"\; state="\{\{state\}\}">t</figure-callout>}}^2}{2}.{\mathrm{\&omega;}}_{\mathrm{ie}}{u}_e\cos L\end{array}\right.$

Step to coarse alignment result

is modified to:

$C_b^n=C_{n^{\&prime;}}^n{C}_b^{n^{\&prime;}}=[I+(\mathrm{\&phi;}\&times;)]C_b^{n^{\&prime;}},$ Wherein $C_{n^{\&prime;}}^n=I+(\mathrm{\&phi;}\&times;)=\left(\begin{array}{ccc}0& -{\mathrm{\&phi;}}_u& {\mathrm{\&phi;}}_n\\ {\mathrm{\&phi;}}_u& 0& -{\mathrm{\&phi;}}_e\\ -{\mathrm{\&phi;}}_n& {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="e"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">e</figure-callout>}}& 0\end{array}\right)$

4) the navigation calculation stage; Carry out the real-Time Compensation of lever arm speed; According to step 33) attitude matrix

that obtains operation strapdown resolves algorithm, and navigation results is provided.

Step 32) in, adopt improvement kalman filter method estimating system state variable:

$\left\{\begin{array}{c}{X}_i(k+1)=X_i\left(k\right)+K_i\left(k\right)e_i\left(k\right)\\ K_i\left(k\right)=P_i\left(k\right)H^T\left(k\right){\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}}^{-1}\\ P_i(k+1)=P_i\left(k\right)-K_i\left(k\right)\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}{K}_i^T\left(k\right),i=e,n;k=\mathrm{0,1,2}\&CenterDot;\&CenterDot;\&CenterDot;\\ R_i(k+1)=R_i\left(k\right)+(e_i^2\left(k\right)-R_i\left(k\right))/(k+1)\\ e_i\left(k\right)=\mathrm{\&Delta;}{V}_i\left(k\right)-H\left(k\right)X_i\left(k\right)\end{array}\right.$

The corresponding expression of i system state variables X _e, X _nIn subscript e, n, original state variable X _i(0), original state estimation error variance battle array P _i(0) and initial observation noise variance intensity R _i(0) value all can be chosen wantonly,

Following formula is estimated system state variables X with recursive algorithm _e, X _n

Step 32), 33) and 4) in; The real-Time Compensation of lever arm speed is: carry out strapdown when resolving; After each Velocity Updating, rejecting the velocity information of the lever arm speed of current time after upgrading, the lever arm speed that is caused by lever arm effect calculates according to the lever arm rate pattern:

$\mathrm{\&delta;}{v}_g={\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;r=\left(\begin{array}{c}{\mathrm{\&omega;}}_{\mathrm{iby}}^b\&CenterDot;r_z-{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\&CenterDot;r_y\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b\&CenterDot;r_x-{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\&CenterDot;r_z\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\&CenterDot;r_y-{\mathrm{\&omega;}}_{\mathrm{iby}}^b\&CenterDot;r_x\end{array}\right).$

Step 33) in; Residual interference speed is carried out the single compensation method is: after the fine alignment phase one finished, the residual interference speed that estimates was normal value, and the fine alignment subordinate phase at the beginning; The disturbance velocity of remnants is rejected from the velocity information that strapdown calculates, and method is:

$\left\{\begin{array}{c}{V^{\&prime;}}_e=V_e-V_{\mathrm{de}}\\ {V^{\&prime;}}_n=V_n-V_{\mathrm{dn}}\end{array}\right.$

In the formula: V ' _e, V ' _nEast orientation and the north orientation speed after the residual interference speed is rejected in expression; V _e, V _nEast orientation and north orientation speed that the expression strapdown resolves; V _De, V _DnExpression east orientation and north orientation residual interference speed.

The part research report relevant with the present invention also arranged at present, and parameter identification method fine alignment wherein of the present invention is with reference to following two pieces of articles: 1, SINS improves parameter identification initial alignment method, Chinese inertial technology journal, 2010,18 (5); 2, the fast and accuracy alignment method of SINS on swaying base, BJ University of Aeronautics & Astronautics's journal, 2009,35 (1); The lever arm rate pattern is with reference to following patent: 3, number of patent application is CN201010270972.4, and name is called " eliminating the alignment methods of carrier SINS lever arm effect error under water ".The present invention is different from prior art and is characterised in that: 1, the fine alignment process is divided into two stages, the phase one is estimated residual interference speed, subordinate phase to residual interference speed single compensation after, estimate misalignment, the result revises to coarse alignment.2, in the fine alignment stage, mutually combine to the real-Time Compensation of lever arm effect with to the single compensation of residual interference speed, can improve the compensation efficient of lever arm effect.3, the error source of labor residual interference speed has designed the parameter identification method fine alignment algorithm of expansion variable targetedly, the residual interference speed in the system that estimates that can be relatively accurate.4, in whole alignment procedures, keep real-Time Compensation to lever arm speed.

The present invention proposes estimation and the compensation method that a kind of SINS rocks the velocity error that fine alignment and navigation stage under the pedestal condition cause by lever arm effect; Purpose is to estimate that lever arm effect compensates to the influence of system and by certain strategy, to improve the precision of fine alignment and navigation calculation.Advantage of the present invention is: adopt the parameter identification method fine alignment algorithm of expansion variable, calculated amount is little, and precision is high; Can estimate the system residual disturbance velocity after dynamics compensates lever arm speed; The fine alignment algorithm is divided into two stages, and the phase one finishes the back residual interference speed is carried out single compensation, can improve subordinate phase fine alignment precision; In the navigation calculation stage lever arm speed is carried out real-Time Compensation, can improve the navigation calculation precision.

Description of drawings

Fig. 1 is a process flow diagram of the present invention.

Fig. 2 is the lever arm effect schematic diagram.

Fig. 3 is that lever arm effect of the present invention is estimated and the backoff algorithm process flow diagram.

Fig. 4 is the misalignment curve map that the parameter identification method fine alignment algorithm of first group of expansion variable of the present invention is estimated.

Fig. 5 is the residual interference speed curve diagram that the parameter identification method fine alignment algorithm of first group of expansion variable of the present invention is estimated.

Fig. 6 is the misalignment curve map that the parameter identification method fine alignment algorithm of second group of expansion variable of the present invention is estimated.

Fig. 7 is the residual interference speed curve diagram that the parameter identification method fine alignment algorithm of second group of expansion variable of the present invention is estimated.

Embodiment

Do detailed description in the face of implementation process of the present invention down, flow process is as shown in Figure 1.

At first define coordinate system.Definition ' sky, northeast ' coordinate system is a navigation coordinate system, is designated as n system; Carrier coordinate system b system is an initial point with the carrier center of gravity, and the X axle points to right along transverse axis, before the Y axle points to along the longitudinal axis, on Z axle vertical carrier points to; Terrestrial coordinate system e system, coordinate origin is point in the earth's core, and the X axle is the plane under the line, points to the first meridian, and the Y axle is vertical with the X axle, and also under the line in the plane, the Z axle is confirmed through right hand rule, sensing earth rotation direction of principal axis.Geocentric inertial coordinate system i system, this coordinate system overlaps at initial time and terrestrial coordinate system.

The output data of gathering inertial measurement cluster after the step 1) SINS start preheating;

Step 2) carries out coarse alignment; Obtain rough initial attitude matrix

because rapidity is the performance index that inertial navigation system is aimed at; Move 3-5 minute coarse alignment algorithm, can obtain rough attitude matrix;

Step 3) divides two stages to accomplish the fine alignment process on the coarse alignment basis, and its flow process is as shown in Figure 3;

Step 31) sets up the parameter identification method fine alignment mathematical model of expansion variable, tectonic system equation and observation equation;

At first set up the parameter identification method mathematical model of expansion variable;

Rocking on the pedestal, the misalignment Changing Pattern between the navigation system of coarse alignment definite navigation system and theory is following:

$\left\{\begin{array}{c}{\mathrm{\&phi;}}_e={\mathrm{\&phi;}}_{e0}+u_e.t+\frac{{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="HDA0000145898280000022.png,HDA0000145898280000031.png"\; state="\{\{state\}\}">t</figure-callout>}}^2}{2}.{\mathrm{\&omega;}}_{\mathrm{Ie}}(u_n\mathrm{Sin}L-u_u\mathrm{Cos}L)\\ {\mathrm{\&phi;}}_n={\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">n</figure-callout>}0}+u_n.t-\frac{{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="HDA0000145898280000022.png,HDA0000145898280000031.png"\; state="\{\{state\}\}">t</figure-callout>}}^2}{2}.{\mathrm{\&omega;}}_{\mathrm{Ie}}{u}_e\mathrm{Sin}L\\ {\mathrm{\&phi;}}_u={\mathrm{\&phi;}}_{u0}+u_u.t+\frac{{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="HDA0000145898280000022.png,HDA0000145898280000031.png"\; state="\{\{state\}\}">t</figure-callout>}}^2}{2}.{\mathrm{\&omega;}}_{\mathrm{Ie}}{u}_e\mathrm{Cos}L\end{array}\right.$ Wherein $\left\{\begin{array}{c}{u}_e={\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">n</figure-callout>}0}{\mathrm{\&omega;}}_{\mathrm{Ie}}\mathrm{Sin}L-{\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="u"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">u</figure-callout>}0}{\mathrm{\&omega;}}_{\mathrm{Ie}}\mathrm{Cos}L-{\mathrm{\&epsiv;}}_e\\ u_n=-{\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="e"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">e</figure-callout>}0}{\mathrm{\&omega;}}_{\mathrm{Ie}}\mathrm{Sin}L-{\mathrm{\&epsiv;}}_n\\ u_u={\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="e"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">e</figure-callout>}0}{\mathrm{\&omega;}}_{\mathrm{Ie}}\mathrm{Cos}L-{\mathrm{\&epsiv;}}_u\end{array}\right.$

Specific force equation on the equivalent level direction is:

$\left\{\begin{array}{c}{f_e}^{n^{\&prime;}}=-g.{\mathrm{\&phi;}}_n+f_{\mathrm{de}}+{\&dtri;}_e\\ {f_n}^{n^{\&prime;}}=g.{\mathrm{\&phi;}}_e+f_{\mathrm{dn}}+{\&dtri;}_n\end{array}\right.$

In the formula: f _De, f _DnBe respectively the disturbing acceleration of equivalent east orientation and equivalent north orientation.Can find out from following formula, owing to there is misalignment φ _e, φ _n, φ _u, cause the terrestrial gravitation acceleration information g that has been coupled in the ratio force information of horizontal direction.Specific force on the horizontal direction is quadratured in the time period in [0, t], obtain the speed of accumulation in this time period.Because carrier is rocking on the pedestal, wireless motion, so this velocity amplitude also is a speed error value, its expression formula is:

$\left\{\begin{array}{c}\mathrm{\&Delta;}{V}_e=({\&dtri;}_e-g.{\mathrm{\&phi;}}_{n0})t-\frac{{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="HDA0000145898280000022.png,HDA0000145898280000031.png"\; state="\{\{state\}\}">t</figure-callout>}}^2}{2}g{u}_n+\frac{{\mathrm{<figure-callout\; id="3"\; label="t"\; filenames="HDA0000145898280000022.png,HDA0000145898280000031.png"\; state="\{\{state\}\}">t</figure-callout>}}^3}{6}g{\mathrm{\&omega;}}_{\mathrm{ie}}{u}_e\sin L+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\&Delta;}{V}_n=({\&dtri;}_n+g.{\mathrm{\&phi;}}_{e0})t+\frac{{\mathrm{<figure-callout\; id="2"\; label="t"\; filenames="HDA0000145898280000022.png,HDA0000145898280000031.png"\; state="\{\{state\}\}">t</figure-callout>}}^2}{2}g{u}_e+\frac{{\mathrm{<figure-callout\; id="3"\; label="t"\; filenames="HDA0000145898280000022.png,HDA0000145898280000031.png"\; state="\{\{state\}\}">t</figure-callout>}}^3}{6}g{\mathrm{\&omega;}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)+V_{\mathrm{dn}}++V_{\mathrm{sn}}\end{array}\right.$

When SINS installation site during not at swing center, lever arm effect can impact east orientation and north orientation speed, and its principle is as shown in Figure 2.In the top formula,

Represent the normal value biasing of accelerometer equivalence east orientation and north orientation respectively; φ _E0, φ _N0, φ _U0Represent initial misalignment; ε _e, ε _n, ε _uExpression equivalence east, north, sky are to gyroscope constant value drift; ω _IeExpression earth rotation angular speed; L representes local latitude; G representes the terrestrial gravitation acceleration; V _Se, V _SnRepresent east orientation and north orientation random disturbance speed respectively, V _De, V _DnExpression east orientation and north orientation residual interference speed are normal value; Δ V _e, Δ V _nRepresent east orientation and north orientation velocity error respectively; Be that the speed resolved of SINS is rejected the poor of velocity amplitude that value and outside reference after the lever arm speed provide; Rocking under the pedestal condition; The speed that outside reference provides is 0m/s, and the process of from the speed that SINS resolves, rejecting lever arm speed is the real-Time Compensation of lever arm speed.If lever arm length vector is r=(r _x, r _y, r _z), the lever arm rate pattern that is caused by lever arm effect is:

$\mathrm{\&delta;}{v}_g={\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;r=\left(\begin{array}{c}{\mathrm{\&omega;}}_{\mathrm{iby}}^b\&CenterDot;r_z-{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\&CenterDot;r_y\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b\&CenterDot;r_x-{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\&CenterDot;r_z\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\&CenterDot;r_y-{\mathrm{\&omega;}}_{\mathrm{iby}}^b\&CenterDot;r_x\end{array}\right)$

In the formula: δ v _gExpression lever arm speed; R=(r _x, r _y, r _z) indication rod arm lengths vector; This value is calculated in advance and is set into system according to the project organization and the navigational system installation site of carrier; In the practical application, lever arm length vector can depart from this value because of factors such as carrier deflection deformation, load distribution variations, and then produces error;

Represent the gyroscope output angle speed on three directions; Therefore labor of the present invention the error source of residual interference speed, said residual interference speed comprises: the random disturbance speed that system exists; The lever arm speed calculation error that, carrier inaccurate because of the lever arm linear measure longimetry exists the factors such as angular speed existence interference of deflection deformation, gyroscope survey to cause; Rocking under the pedestal condition, strapdown resolves in the speed of initial time and has lever arm speed, and when the operation strapdown resolved algorithm, initial velocity press 0m/s processing, the residual speed that causes; Behind the coarse alignment, there is error in initial attitude matrix, when the operation strapdown resolves algorithm, and the velocity error of bringing.

Parameter identification method mathematical model tectonic system equation and observation equation according to expansion variable:

As observed quantity, be rewritten into following form to the parameter identification method mathematical model of expansion variable with SINS medium velocity error:

$\left\{\begin{array}{c}\mathrm{\&Delta;}{V}_e=a_{1e}\left(\mathrm{KT}\right)+a_{2e}{\left(\mathrm{KT}\right)}^2+a_{3e}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\&Delta;}{V}_n=a_{1n}\left(\mathrm{KT}\right)+a_{2n}{\left(\mathrm{KT}\right)}^2+a_{3n}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{dn}}+V_{\mathrm{sn}}\end{array}\right.$

In the formula: T is the sensor data samples cycle of SINS;

$\left\{\begin{array}{c}{a}_{1e}=({\&dtri;}_e-g.{\mathrm{\&phi;}}_{n0}),a_{2e}=-\frac{1}{2}g{u}_n,a_{3e}=\frac{1}{6}g{\mathrm{\&omega;}}_{\mathrm{ie}}{u}_e\sin L\\ a_{1n}=({\&dtri;}_n+g.{\mathrm{\&phi;}}_{e0}),a_{2n}=\frac{1}{2}g{u}_e,a_{3n}=\frac{1}{6}g{\mathrm{\&omega;}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\end{array}\right.$

East orientation and north orientation velocity error Δ V _e, Δ V _nAs observed quantity, a _1e, a _2e, a _3e, V _De, a _1n, a _2n, a _3n, V _DnRegard as and treat identified parameters, tectonic system equation and observation equation:

Identified parameters is treated in definition, and promptly system state variables is:

$X_e=\left[\begin{array}{c}{a}_{1e}\\ a_{2e}\\ a_{3e}\\ V_{\mathrm{de}}\end{array}\right],$ $X_n=\left[\begin{array}{c}{a}_{1n}\\ a_{2n}\\ a_{3n}\\ V_{\mathrm{dn}}\end{array}\right]$

According to the analysis of front, list system equation and observation equation:

$\left\{\begin{array}{c}{X}_e(k+1)=X_e\left(k\right)\\ \mathrm{\&Delta;}{V}_e\left(k\right)=H\left(k\right)X_e\left(k\right)+V_{\mathrm{ge}}\left(k\right)\end{array}\right.,$ $\left\{\begin{array}{c}{X}_n(k+1)=X_n\left(k\right)\\ \mathrm{\&Delta;}{V}_n\left(k\right)=H\left(k\right)X_n\left(k\right)+V_{\mathrm{gn}}\left(k\right)\end{array}\right.$

In the formula: observing matrix H (k)=[kT, (kT) ², (kT) ³, 1]; V _Ge(k), V _Gn(k) the speed observation noise of expression east orientation and north orientation, its variance intensity is R _e, R _n

Step 32) the fine alignment phase one; According to the coarse alignment result, the operation strapdown resolves program, carries out the real-Time Compensation of lever arm speed; Simultaneously according to step 31) system equation and the observation equation estimating system state variable set up, the 4th component of state variable is residual interference speed.The method of estimated state variable is a lot, can pass through least square method recursion or kalman filter method, and the present invention adopts improvement kalman filter method estimated state variable.Based on the consideration of rapidity and precision, this process generally was made as 3-5 minute.

The method of the real-Time Compensation of the speed of lever arm described in the present invention is: carry out strapdown when resolving; After each Velocity Updating; Rejecting the velocity information of the lever arm speed of current time after upgrading, the lever arm speed that is caused by lever arm effect calculates according to the lever arm rate pattern:

$\mathrm{\&delta;}{v}_g={\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;r=\left(\begin{array}{c}{\mathrm{\&omega;}}_{\mathrm{iby}}^b\&CenterDot;r_z-{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\&CenterDot;r_y\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b\&CenterDot;r_x-{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\&CenterDot;r_z\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\&CenterDot;r_y-{\mathrm{\&omega;}}_{\mathrm{iby}}^b\&CenterDot;r_x\end{array}\right)$

State variable X _e, X _nValue can calculate as recursion through least square method.In recursive process, parameter is gradually to the true value convergence, and this process is from observation information, to refine the concentration process of estimated value, and along with recursion is deep more, concentration process is effective more, and estimated value is also more near actual value.

The present invention has adopted improvement kalman filter method to estimate that algorithm for estimating is following to the estimation of state variable:

$\left\{\begin{array}{c}{X}_i(k+1)=X_i\left(k\right)+K_i\left(k\right)e_i\left(k\right)\\ K_i\left(k\right)=P_i\left(k\right)H^T\left(k\right){\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}}^{-1}\\ P_i(k+1)=P_i\left(k\right)-K_i\left(k\right)\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}{K}_i^T\left(k\right),i=e,n;k=\mathrm{0,1,2}\&CenterDot;\&CenterDot;\&CenterDot;\\ R_i(k+1)=R_i\left(k\right)+(e_i^2\left(k\right)-R_i\left(k\right))/(k+1)\\ e_i\left(k\right)=\mathrm{\&Delta;}{V}_i\left(k\right)-H\left(k\right)X_i\left(k\right)\end{array}\right.$

The corresponding system state variables X of i _e, X _nIn subscript e, n.Original state variable X _i(0), original state variance of estimaion error battle array P _i(0) and the variance intensity R of initial observation noise _i(0) value all can be chosen wantonly.Might as well get X _i(0)=0; P _i(0)=and I α, α is positive scalar arbitrarily; R _i(0)=0.2.In the algorithm, calculation of filtered gain battle array K _i(k) utilized new breath e the time _i(k), the benefit of doing like this is: even the observation noise statistical property in the system is unknown, also can be according to the adaptive observation noise that calculates current time of new breath value, and upgrade the filter gain battle array according to observation noise, help the quick convergence of algorithm.

Can estimate system state variables X to following formula with recursive algorithm _e, X _n

Step 33) fine alignment subordinate phase: residual interference speed is carried out single compensation; Then, continue the lever arm speed real-Time Compensation of operation fine alignment phase one and, carry out initial misalignment and estimate, after the initial misalignment convergence to be estimated, estimated value φ to the algorithm for estimating of system state variables _E0, φ _N0, φ _U0Calculate the misalignment φ of current time in the substitution misalignment Changing Pattern model _e, φ _n, φ _uThe misalignment information of utilizing current time is to the coarse alignment result

Carry out a step and revise, obtain the attitude matrix of current time

Pass through attitude matrix

Extract position angle H, pitch angle P and roll angle R, fine alignment is accomplished.Based on the consideration of rapidity and precision, this process generally was made as 5 minutes.Specific as follows:

After system state variables is estimated out, calculate u _e, u _n, u _u, φ _E0, φ _N0And φ _U0:

$u_e=\frac{2a_{2n}}{g},$ $u_n=-\frac{2a_{2e}}{g},$ $u_u=-\frac{6a_{3n}}{g{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L}-\frac{2a_{2e}}{g}\tan L$

${\mathrm{\&phi;}}_{e0}=\frac{a_{1n}}{g},$ ${\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">n</figure-callout>}0}=-\frac{a_{1e}}{g},$ ${\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="u"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">u</figure-callout>}0}={\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">n</figure-callout>}0}.\tan L-\frac{u_e+{\mathrm{\&epsiv;}}_e}{{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L}$

Because equivalent east orientation gyroscopic drift ε _eThe unknown, the following formula approximate treatment is used to the initial value of misalignment in the sky:

${\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="u"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">u</figure-callout>}0}\&ap;{\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">n</figure-callout>}0}.\tan L-\frac{u_e}{{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L}$

Owing in calculating, ignored the constant value drift of sensor, the initial value of misalignment to be estimated to have error, the expression formula of error amount is:

$\left\{\begin{array}{c}\mathrm{\&delta;}{\mathrm{\&phi;}}_{e0}=-\frac{{\&dtri;}_n}{g}\\ \mathrm{\&delta;}{\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">n</figure-callout>}0}=\frac{{\&dtri;}_e}{g}\\ \mathrm{\&delta;}{\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="u"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">u</figure-callout>}0}=-\frac{{\mathrm{\&epsiv;}}_e}{{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L}\end{array}\right.$

After coarse alignment finished, the navigation of being set up by navigational computer was that n ' is that n exists misalignment φ with desirable navigation _e, φ _n, φ _u, they all are little angles, vector φ=(φ _eφ _nφ _u) can be regarded as n and be tied to the equivalent rotating vector that n ' is, n is tied to the attitude transition matrix that n ' is and can be expressed from the next:

$C_{n^{\&prime;}}^n=I+(\mathrm{\&phi;}\&times;)=\left(\begin{array}{ccc}0& -{\mathrm{\&phi;}}_u& {\mathrm{\&phi;}}_n\\ {\mathrm{\&phi;}}_u& 0& -{\mathrm{\&phi;}}_e\\ -{\mathrm{\&phi;}}_n& {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="e"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">e</figure-callout>}}& 0\end{array}\right)$

Step to coarse alignment result

is modified to:

$C_b^n=C_{n^{\&prime;}}^n{C}_b^{n^{\&prime;}}=[I+(\mathrm{\&phi;}\&times;)]C_b^{n^{\&prime;}},$ Wherein $C_{n^{\&prime;}}^n=I+(\mathrm{\&phi;}\&times;)=\left(\begin{array}{ccc}0& -{\mathrm{\&phi;}}_u& {\mathrm{\&phi;}}_n\\ {\mathrm{\&phi;}}_u& 0& -{\mathrm{\&phi;}}_e\\ -{\mathrm{\&phi;}}_n& {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="e"\; filenames="HDA0000145898280000032.png"\; state="\{\{state\}\}">e</figure-callout>}}& 0\end{array}\right)$

Residual interference speed is carried out the single compensation method is: after the fine alignment phase one finished, the residual interference speed that estimates was normal value, and the fine alignment subordinate phase is rejected the disturbance velocity of remnants at the beginning from the velocity information that strapdown calculates, and method is:

$\left\{\begin{array}{c}{V^{\&prime;}}_e=V_e-V_{\mathrm{de}}\\ {V^{\&prime;}}_n=V_n-V_{\mathrm{dn}}\end{array}\right.$

In the formula: V ' _e, V ' _nEast orientation and the north orientation speed after the residual interference speed is rejected in expression; V _e, V _nEast orientation and north orientation speed that the expression strapdown resolves; V _De, V _DnExpression east orientation and north orientation residual interference speed.

The step 4) navigation calculation stage; Carry out the real-Time Compensation of lever arm speed; According to step 33) attitude matrix that obtains operation strapdown resolves algorithm, and navigation results is provided.

Method of the present invention is carried out emulation experiment:

Simulated conditions: carrier is under the horizontal jitter pedestal condition, and shaking amplitude and frequency are respectively: 0.17 °/1Hz; Consider the lever arm influence in the system, lever arm length is respectively: 0.456m ,-0.456m, 22.546m; The gyroscope constant value drift of three directions is 0.15 °/h, and the Jia Biaochang value is biased to 0.11mg, and latitude is 31183 °, and attitude of carrier is: 2 ° of the angles of pitch, 3 ° of roll angles.After coarse alignment finishes, be the parameter identification method fine alignment that expansion variable is carried out in 90 ° position in the course,, carried out two groups of experiments in order to verify estimated accuracy to the system residual disturbance velocity.Fine alignment phase one algorithm is carried out in first group of experiment, moves 5 minutes; Fine alignment subordinate phase algorithm is carried out in second group of experiment, moves 5 minutes.

First group of experiment: there is initial velocity error in the system of setting up departments, promptly has residual interference speed, after the parameter identification method fine alignment program of operation expansion variable, estimates initial interference speed, alignment result such as Fig. 4, shown in Figure 5:

First group of experimental result: the coarse alignment result is: course error δ H=-38.76 ', δ P=0.61 ', δ R=-0.21 ', attitude matrix behind the aligning and theoretical attitude matrix apart from dis (C)=0.016.The accurate result of parameter identification method is: course error δ H=-42 ', δ P=0.17 ', δ R=-20 ', attitude matrix behind the aligning and theoretical attitude matrix apart from dis (C)=0.017.Because during first group of experiment, there is initial disturbance velocity in system, promptly has residual interference speed, causes the fine alignment process to have disturbing effect.The east orientation disturbance velocity that this method is estimated is 0.4138m/s, and the north orientation disturbance velocity is approximately-0.4245m/s.

Second group of experiment, the disturbance velocity that on the basis of last group of experiment, comes out estimation carries out single compensation in the fine alignment subordinate phase, alignment result such as Fig. 6, shown in Figure 7:

Second group of accurate result of experiment parameter identification method is: course error δ H=-38.94 ', and δ P=-0.15 ', δ R=-0.20 ', attitude matrix behind the aligning and theoretical attitude matrix apart from dis (C)=0.0016, compare coarse alignment and phase one fine alignment result, precision increases.Because second group of emulation experiment tested the initial interference speed of estimating to first group and compensated, misalignment evaluated error very rapid convergence has improved alignment precision to smaller value.According to experimental result, explain that the initial lever arm disturbance velocity of first group of experiment estimation is effective.In second group of experiment, because residual interference speed is estimated and compensated, the disturbance velocity of the remnants of theory is in the system: east orientation 0m/s, north orientation 0m/s.In this group test, disturbance velocity remaining in the system is estimated estimated result is: the east orientation disturbance velocity is 0.00045m/s, and the north orientation disturbance velocity is approximately-and 0.0006m/s and theoretical 0m/s, 0m/s are very approaching, and estimated accuracy surpasses 95%.

Claims (5)

1. an estimation and compensation method that SINS rocks lever arm effect under the pedestal condition is characterized in that rocking under the pedestal condition, estimates that lever arm effect to the influence of SINS and compensate, may further comprise the steps:

1) SINS start preheating, the output data of gathering inertial measurement cluster;

2) carry out coarse alignment, obtain rough initial attitude matrix

3) on the coarse alignment basis, divide two stages to accomplish the fine alignment process;

31) set up the parameter identification method fine alignment mathematical model of expansion variable, tectonic system equation and observation equation; The parameter identification method fine alignment mathematical model of expansion variable:

$\left\{\begin{array}{c}\mathrm{\&Delta;}{V}_e=({\&dtri;}_e-g.{\mathrm{\&phi;}}_{n0})t-\frac{t^2}{2}g{u}_n+\frac{t^3}{6}g{\mathrm{\&omega;}}_{\mathrm{ie}}{u}_e\sin L+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\&Delta;}{V}_n=({\&dtri;}_n+g.{\mathrm{\&phi;}}_{e0})t+\frac{t^2}{2}g{u}_e+\frac{t^3}{6}g{\mathrm{\&omega;}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)+V_{\mathrm{dn}}++V_{\mathrm{sn}}\end{array}\right.$

Wherein $\left\{\begin{array}{c}{u}_e={\mathrm{\&phi;}}_{n0}{\mathrm{\&omega;}}_{\mathrm{Ie}}\mathrm{Sin}L-{\mathrm{\&phi;}}_{u0}{\mathrm{\&omega;}}_{\mathrm{Ie}}\mathrm{Cos}L-{\mathrm{\&epsiv;}}_e\\ u_n=-{\mathrm{\&phi;}}_{e0}{\mathrm{\&omega;}}_{\mathrm{Ie}}\mathrm{Sin}L-{\mathrm{\&epsiv;}}_n\\ u_u={\mathrm{\&phi;}}_{e0}{\mathrm{\&omega;}}_{\mathrm{Ie}}\mathrm{Cos}L-{\mathrm{\&epsiv;}}_u\end{array}\right.$

In the formula:

Represent the normal value biasing of accelerometer equivalence east orientation and north orientation respectively; φ _E0, φ _N0, φ _U0Represent initial misalignment; ε _e, ε _n, ε _uExpression equivalence east, north, sky are to gyroscope constant value drift; ω _IeExpression earth rotation angular speed; L representes local latitude; G representes the terrestrial gravitation acceleration; V _Se, V _SnRepresent east orientation and north orientation random disturbance speed respectively, V _De, V _DnExpression east orientation and north orientation residual interference speed are normal value; Δ V _e, Δ V _nRepresent east orientation and north orientation velocity error respectively; Be that the speed resolved of SINS is rejected the poor of velocity amplitude that value and outside reference after the lever arm speed provide; Rocking under the pedestal condition; The speed that outside reference provides is 0m/s, and the process of from the speed that SINS resolves, rejecting lever arm speed is the real-Time Compensation of lever arm speed, and the lever arm speed that is caused by lever arm effect calculates according to the lever arm rate pattern:

$\mathrm{\&delta;}{v}_g={\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;r=\left(\begin{array}{c}{\mathrm{\&omega;}}_{\mathrm{iby}}^b\&CenterDot;r_z-{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\&CenterDot;r_y\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b\&CenterDot;r_x-{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\&CenterDot;r_z\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\&CenterDot;r_y-{\mathrm{\&omega;}}_{\mathrm{iby}}^b\&CenterDot;r_x\end{array}\right)$

In the formula: δ v _gExpression lever arm speed; R=(r _x, r _y, r _z) indication rod arm lengths vector, this value is calculated in advance and is set into system according to the project organization and the navigational system installation site of carrier, and in the practical application, lever arm length vector can be because factors such as carrier deflection deformation, load distribution variation depart from this value;

Represent the gyroscope output angle speed on three directions;

Said residual interference speed comprises: the random disturbance speed that system exists; The lever arm speed calculation error that, carrier inaccurate because of the lever arm linear measure longimetry exists the factors such as angular speed existence interference of deflection deformation, gyroscope survey to cause; Rocking under the pedestal condition, strapdown resolves in the speed of initial time and has lever arm speed, and when the operation strapdown resolved algorithm, initial velocity press 0m/s processing, the residual speed that causes; Behind the coarse alignment, there is error in initial attitude matrix, when the operation strapdown resolves algorithm, and the velocity error of bringing;

According to the parameter identification method mathematical model of expansion variable, tectonic system equation and observation equation are:

With SINS medium velocity error is observed quantity, is rewritten into following form to the parameter identification method mathematical model of expansion variable:

$\left\{\begin{array}{c}\mathrm{\&Delta;}{V}_e=a_{1e}\left(\mathrm{KT}\right)+a_{2e}{\left(\mathrm{KT}\right)}^2+a_{3e}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{de}}+V_{\mathrm{se}}\\ \mathrm{\&Delta;}{V}_n=a_{1n}\left(\mathrm{KT}\right)+a_{2n}{\left(\mathrm{KT}\right)}^2+a_{3n}{\left(\mathrm{KT}\right)}^3+V_{\mathrm{dn}}+V_{\mathrm{sn}}\end{array}\right.,K=\mathrm{0,1,2},\&CenterDot;\&CenterDot;\&CenterDot;$

In the formula: T is the sensor data samples cycle of SINS;

$\left\{\begin{array}{c}{a}_{1e}=({\&dtri;}_e-g.{\mathrm{\&phi;}}_{n0}),a_{2e}=-\frac{1}{2}g{u}_n,a_{3e}=\frac{1}{6}g{\mathrm{\&omega;}}_{\mathrm{ie}}{u}_e\sin L\\ a_{1n}=({\&dtri;}_n+g.{\mathrm{\&phi;}}_{e0}),a_{2n}=\frac{1}{2}g{u}_e,a_{3n}=\frac{1}{6}g{\mathrm{\&omega;}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\end{array}\right.$

East orientation and north orientation velocity error Δ V _e, Δ V _eAs observed quantity, a _1e, a _2e, a _3e, V _De, a _1n, a _2n, a _3n, V _DnAs treating identified parameters, tectonic system equation and observation equation:

Identified parameters is treated in definition, and promptly system state variables is:

$X_e=\left[\begin{array}{c}{a}_{1e}\\ a_{2e}\\ a_{3e}\\ V_{\mathrm{de}}\end{array}\right],$ $X_n=\left[\begin{array}{c}{a}_{1n}\\ a_{2n}\\ a_{3n}\\ V_{\mathrm{dn}}\end{array}\right]$

List system equation and observation equation:

$\left\{\begin{array}{c}{X}_e(k+1)=X_e\left(k\right)\\ \mathrm{\&Delta;}{V}_e\left(k\right)=H\left(k\right)X_e\left(k\right)+V_{\mathrm{ge}}\left(k\right)\end{array}\right.,$ $\left\{\begin{array}{c}{X}_n(k+1)=X_n\left(k\right)\\ \mathrm{\&Delta;}{V}_n\left(k\right)=H\left(k\right)X_n\left(k\right)+V_{\mathrm{gn}}\left(k\right)\end{array}\right.$

In the formula: observing matrix H (k)=[kT, (kT) ², (kT) ³, V _Ge(k), V _Gn(k) the speed observation noise of expression east orientation and north orientation, its variance intensity is R _e, R _n

32) the fine alignment phase one; According to the coarse alignment result, the operation strapdown resolves program, carries out the real-Time Compensation of lever arm speed; Simultaneously according to step 31) system equation and the observation equation estimating system state variable set up, the 4th component of state variable is residual interference speed;

33) fine alignment subordinate phase: residual interference speed is carried out single compensation; Then, continue the lever arm speed real-Time Compensation of operation fine alignment phase one and, and carry out initial misalignment and estimate, after the initial misalignment convergence to be estimated, estimated value φ to the algorithm for estimating of system state variables _E0, φ _N0, φ _U0Calculate the misalignment φ of current time in the substitution misalignment Changing Pattern model _e, φ _n, φ _uThe misalignment information of utilizing current time is to the coarse alignment result

Carry out a step and revise, obtain the attitude matrix of current time

Pass through attitude matrix

Extract position angle H, pitch angle P and roll angle R, fine alignment is accomplished;

Wherein, according to step 32) estimate that the state variable obtain calculates u _e, u _n, u _u, φ _E0, φ _N0And φ _U0:

$u_e=\frac{2a_{2n}}{g},$ $u_n=-\frac{2a_{2e}}{g},$ $u_u=-\frac{6a_{3n}}{g{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L}-\frac{2a_{2e}}{g}\tan L$

${\mathrm{\&phi;}}_{e0}=\frac{a_{1n}}{g},$ ${\mathrm{\&phi;}}_{n0}=-\frac{a_{1e}}{g},$ ${\mathrm{\&phi;}}_{u0}={\mathrm{\&phi;}}_{n0}.\tan L-\frac{u_e}{{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L}$

According to initial misalignment and misalignment Changing Pattern Model Calculation current time misalignment:

$\left\{\begin{array}{c}{\mathrm{\&phi;}}_e={\mathrm{\&phi;}}_{e0}+u_e.t+\frac{t^2}{2}.{\mathrm{\&omega;}}_{\mathrm{ie}}(u_n\sin L-u_u\cos L)\\ {\mathrm{\&phi;}}_n={\mathrm{\&phi;}}_{n0}+u_n.t-\frac{t^2}{2}.{\mathrm{\&omega;}}_{\mathrm{ie}}{u}_e\sin L\\ {\mathrm{\&phi;}}_u={\mathrm{\&phi;}}_{u0}+u_u.t+\frac{t^2}{2}.{\mathrm{\&omega;}}_{\mathrm{ie}}{u}_e\cos L\end{array}\right.$

Step to coarse alignment result

is modified to:

$C_b^n=C_{n^{\&prime;}}^n{C}_b^{n^{\&prime;}}=[I+(\mathrm{\&phi;}\&times;)]C_b^{n^{\&prime;}},$ Wherein $C_{n^{\&prime;}}^n=I+(\mathrm{\&phi;}\&times;)=\left(\begin{array}{ccc}0& -{\mathrm{\&phi;}}_u& {\mathrm{\&phi;}}_n\\ {\mathrm{\&phi;}}_u& 0& -{\mathrm{\&phi;}}_e\\ -{\mathrm{\&phi;}}_n& {\mathrm{\&phi;}}_e& 0\end{array}\right)$

4) the navigation calculation stage; Carry out the real-Time Compensation of lever arm speed; According to step 33) attitude matrix

that obtains operation strapdown resolves algorithm, and navigation results is provided.

2. a kind of SINS according to claim 1 rocks the estimation and the compensation method of lever arm effect under the pedestal condition, it is characterized in that step 32) in, adopt improvement kalman filter method estimating system state variable:

$\left\{\begin{array}{c}{X}_i(k+1)=X_i\left(k\right)+K_i\left(k\right)e_i\left(k\right)\\ K_i\left(k\right)=P_i\left(k\right)H^T\left(k\right){\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}}^{-1}\\ P_i(k+1)=P_i\left(k\right)-K_i\left(k\right)\{H\left(k\right)P_i\left(k\right)H^T\left(k\right)+R_i(k+1)\}{K}_i^T\left(k\right),i=e,n;k=\mathrm{0,1,2}\&CenterDot;\&CenterDot;\&CenterDot;\\ R_i(k+1)=R_i\left(k\right)+(e_i^2\left(k\right)-R_i\left(k\right))/(k+1)\\ e_i\left(k\right)=\mathrm{\&Delta;}{V}_i\left(k\right)-H\left(k\right)X_i\left(k\right)\end{array}\right.$

The corresponding expression of i system state variables X _e, X _nIn subscript e, n, original state variable X _i(0), original state estimation error variance battle array P _i(0) and initial observation noise variance intensity R _i(0) value all can be chosen wantonly,

Following formula is estimated system state variables X with recursive algorithm _e, X _n

3. a kind of SINS according to claim 1 and 2 rocks the estimation and the compensation method of lever arm effect under the pedestal condition; It is characterized in that step 32), 33) and 4) in; The real-Time Compensation of lever arm speed is: carry out strapdown when resolving; After each Velocity Updating, rejecting the velocity information of the lever arm speed of current time after upgrading, the lever arm speed that is caused by lever arm effect calculates according to the lever arm rate pattern:

$\mathrm{\&delta;}{v}_g={\mathrm{\&omega;}}_{\mathrm{ib}}^b\&times;r=\left(\begin{array}{c}{\mathrm{\&omega;}}_{\mathrm{iby}}^b\&CenterDot;r_z-{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\&CenterDot;r_y\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b\&CenterDot;r_x-{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\&CenterDot;r_z\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\&CenterDot;r_y-{\mathrm{\&omega;}}_{\mathrm{iby}}^b\&CenterDot;r_x\end{array}\right).$

4. a kind of SINS according to claim 1 and 2 rocks the estimation and the compensation method of lever arm effect under the pedestal condition; It is characterized in that step 33) in; Residual interference speed is carried out the single compensation method is: after the fine alignment phase one finished, the residual interference speed that estimates was normal value, and the fine alignment subordinate phase at the beginning; The disturbance velocity of remnants is rejected from the velocity information that strapdown calculates, and method is:

$\left\{\begin{array}{c}{V^{\&prime;}}_e=V_e-V_{\mathrm{de}}\\ {V^{\&prime;}}_n=V_n-V_{\mathrm{dn}}\end{array}\right.$

In the formula: V ' _e, V ' _nEast orientation and the north orientation speed after the residual interference speed is rejected in expression; V _e, V _nEast orientation and north orientation speed that the expression strapdown resolves; V _De, V _DnExpression east orientation and north orientation residual interference speed.

5. a kind of SINS according to claim 3 rocks the estimation and the compensation method of lever arm effect under the pedestal condition; It is characterized in that step 33) in; Residual interference speed is carried out the single compensation method is: after the fine alignment phase one finished, the residual interference speed that estimates was normal value, and the fine alignment subordinate phase at the beginning; The disturbance velocity of remnants is rejected from the velocity information that strapdown calculates, and method is:

$\left\{\begin{array}{c}{V^{\&prime;}}_e=V_e-V_{\mathrm{de}}\\ {V^{\&prime;}}_n=V_n-V_{\mathrm{dn}}\end{array}\right.$

In the formula: V ' _e, V ' _nEast orientation and the north orientation speed after the residual interference speed is rejected in expression; V _e, V _nEast orientation and north orientation speed that the expression strapdown resolves; V _De, V _DnExpression east orientation and north orientation residual interference speed.

Images