CN103557873A - Quick dynamic alignment method - Google Patents

Abstract

The invention belongs to alignment methods and in particular relates to a quick dynamic alignment method. The quick dynamic alignment method comprises the following steps: 1, coarsely aligning, namely 1.1, inputting information, 1.2, calculating ax, ay and az, 1.3, calculating omega x and omega z, 1.4, calculating according to a formula (1), 1.5, calculating according to a formula (2), and 1.6, calculating according to a formula (3); 2, carrying out navigation calculation; 3, calculating an intermediate variable; and 4, aligning. The quick dynamic alignment method has the beneficial effects of being capable of accurately estimating three misalignment angles of a platform system relative to a geographical system and three uncompensated platform drifts (of the platform system) under angular vibration of a carrier so as to achieve the purpose of high-precision self-alignment.

Description

A kind of quick dynamic alignment method

Technical field

The invention belongs to alignment methods, be specifically related to a kind of quick dynamic alignment method.

Background technology

Platform Inertial Navigation System can externally exported before navigation information, must carry out initial alignment.Under quiet pedestal condition, in order to complete initial alignment, need to utilize gravity and earth rotation angular speed information, make platform stage body in Platform Inertial Navigation System right on the course be operated in one or more diverse locations, thereby show that the drift of three gyros is three misalignments of relative Department of Geography with platform.Yet under swaying base condition, due to the angular oscillation of carrier, make inertial platform record gravity and earth rotation angular speed all disturbs, had a strong impact on alignment precision.In order to eliminate these, disturb, need to adopt new filtering method to complete the Platform INS autoregistration under swaying base.

Summary of the invention

The object of the invention is the defect for prior art, a kind of quick dynamic alignment method is provided.

The present invention is achieved in that 1. 1 kinds of quick dynamic alignment methods, it is characterized in that, comprises the steps:

Step 1: coarse alignment

Step 1.1: input message

Need the information of input to comprise following three classes

(a) accelerometer data

$b_{\mathrm{xk}}^{\left(2\right)},b_{\mathrm{yk}}^{\left(2\right)},b_{\mathrm{zk}}^{\left(2\right)}(k=1,...,N^{\left(2\right)})$

Above-mentioned xyz refers to the direction of acceleration, and subscript k represents the data of k sampling instant, and the footmark in the upper right corner (2) refers to position two, N ⁽²⁾data altogether in the second place.

(b) revolve parameter certificate

J represents position, and the application has two positions, and the footmark in the Er， lower right corner, Yi He position, position 1,2,3 represents the data that different sensors provide, and needs input in each position, the zero hour revolve parameter certificate

The meaning of all footmarks and front identical, described T ₁refer to the finish time, T ₁/ 2 refer in the middle of constantly, need input in each position, constantly middle and finish time revolve parameter certificate.

(c) accelerometer parameter

n _0x,n _0y,n _0z,M _x,M _y,M _z,△ _yx,△ _zx,△ _zy

Described n _0x, n _0y, n _0zfor accelerometer bias, M _x, M _y, M _zfor accelerometer calibration factor, △ _yx, △ _zx, △ _zyfor accelerometer alignment error.

Above-mentioned accelerometer parameter all can directly obtain.

Step 1.2: calculate a _x, a _y, a _z

$a_x=\frac{1}{T}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}\left(b_{\mathrm{xk}}^{\left(2\right)}{M}_x\right)-n_{0x}$

$a_y=\frac{1}{T}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}(b_{\mathrm{yk}}^{\left(2\right)}{M}_y-b_{\mathrm{xk}}^{\left(2\right)}{M}_x{\mathrm{\Δ}}_{\mathrm{yx}})-n_{0y}$

$a_z=\frac{1}{T}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}(b_{\mathrm{zk}}^{\left(2\right)}{M}_z-b_{\mathrm{xk}}^{\left(2\right)}{M}_x{\mathrm{\Δ}}_{\mathrm{zx}}-b_{\mathrm{yk}}^{\left(2\right)}{M}_y{\mathrm{\Δ}}_{\mathrm{zy}})-n_{0z}$

In formula:

N _0x, n _0y, n _0zfor accelerometer bias; M _x, M _y, M _zfor accelerometer calibration factor;

Δ _yx, Δ _zx, Δ _zyfor accelerometer alignment error.

Step 1.3: calculate ω _x, ω _z

In formula:

for T ₁the angle in/2 moment.

Step 1.4: calculate

$C_{\mathrm{gp}}^{\left(2\right)}(T_1/2)=\left(\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right)$

In formula:

$c_{21}=\frac{a_x}{\sqrt{a_x^2+a_y^2+a_z^2}}$

$c_{22}=\frac{a_y}{\sqrt{a_x^2+a_y^2+a_z^2}}$

$c_{23}=\frac{a_z}{\sqrt{a_x^2+a_y^2+a_z^2}}$

c ₁₁=c ₂₂c ₃₃-c ₃₂c ₂₃

c ₁₂=c ₃₁c ₂₃-c ₂₁c ₃₃

c ₁₃=c ₂₁c ₃₂-c ₃₁c ₂₂

Wherein Ω is earth rotation angular speed.

Step 1.5: calculate

$C_{\mathrm{mg}}^{\left(2\right)}(T_1/2)=C_{\mathrm{mp}}^{\left(2\right)}(T_1/2)C_{\mathrm{pg}}^{\left(2\right)}(T_1/2)$

In formula

$C_{\mathrm{mp}}=\left(\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right)$ Here

extremely

for T ₁the angle in/2 moment.

Step 1.6: calculate

$C_{\mathrm{gp}}^{\left(j\right)}\left(0\right)={\left[C_{\mathrm{pm}}^{\left(j\right)}\left(0\right)C_{\mathrm{mg}}^{\left(2\right)}(T_1/2)\right]}^T(j=\mathrm{1,2})$

output data for this step.

A kind of quick dynamic alignment method as above wherein, increases following step after step 1,

Step 2: navigation calculation

Step 2.1: input

The parameter that the parameter that this step needs has been inputted in step 1, also comprise

(a) platform drift and accelerometer parameter

ω _0x, ω _0y, ω _0zthe irrelevant drift of platform and acceleration, these three parameters are the intrinsic parameter of platform,

ω _xx, ω _xz, ω _yx, ω _yy, ω _zx, ω _zzplatform and acceleration associated drift, these six parameters are the input that sensor measurement obtains;

N _0x, n _0y, n _0z, M _x, M _y, M _zaccelerometer bias and calibration factor, these six parameters are the intrinsic parameter of accelerometer;

Each position

(

), this parameter is that step 1 calculates;

The linear velocity of pedestal these three obtain for sensor measurement.

Step 2.2: calculate ω _xk-1, ω _xk, ω _yk-1, ω _yk, ω _zk-1, ω _zk

By following formula, calculate

C _ip0=C _gp（0)

$C_{\mathrm{ipk}}=C_{\mathrm{ipk}-1}\left[\begin{array}{ccc}1& {-\mathrm{\ω}}_{\mathrm{zk}-1}\mathrm{\Δt}& {\mathrm{\ω}}_{\mathrm{yk}-1}\mathrm{\Δt}\\ {\mathrm{\ω}}_{\mathrm{zk}-1}\mathrm{\Δt}& 1& {-\mathrm{\ω}}_{\mathrm{xk}-1}\mathrm{\Δt}\\ {-\mathrm{\ω}}_{\mathrm{yk}-1}\mathrm{\Δt}& {\mathrm{\ω}}_{\mathrm{xk}-1}\mathrm{\Δt}& 1\end{array}\right]$

${\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{xk}}\\ {\mathrm{\ω}}_{\mathrm{yk}}\\ {\mathrm{\ω}}_{\mathrm{zk}}\end{array}\right]}^T={\left[\begin{array}{c}{\mathrm{\ω}}_{0x}\\ {\mathrm{\ω}}_{0y}\\ {\mathrm{\ω}}_{0z}\end{array}\right]}^T+{\left[\begin{array}{c}{C}_{\mathrm{gp}21k}\\ C_{\mathrm{gp}22k}\\ C_{\mathrm{gp}23k}\end{array}\right]}^T\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{xx}}& {\mathrm{\ω}}_{\mathrm{yx}}& {\mathrm{\ω}}_{\mathrm{zx}}\\ {\mathrm{\ω}}_{\mathrm{xy}}& {\mathrm{\ω}}_{\mathrm{yy}}& {\mathrm{\ω}}_{\mathrm{zy}}\\ {\mathrm{\ω}}_{\mathrm{xz}}& {\mathrm{\ω}}_{\mathrm{yz}}& {\mathrm{\ω}}_{\mathrm{zz}}\end{array}\right]$

${\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{xk}-1}\\ {\mathrm{\ω}}_{\mathrm{yk}-1}\\ {\mathrm{\ω}}_{\mathrm{zk}-1}\end{array}\right]}^T={\left[\begin{array}{c}{\mathrm{\ω}}_{0x}\\ {\mathrm{\ω}}_{0y}\\ {\mathrm{\ω}}_{0z}\end{array}\right]}^T+{\left[\begin{array}{c}{C}_{\mathrm{gp}21k-1}\\ C_{\mathrm{gp}22k-1}\\ C_{\mathrm{gp}23k-1}\end{array}\right]}^T\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{xx}}& {\mathrm{\ω}}_{\mathrm{yx}}& {\mathrm{\ω}}_{\mathrm{zx}}\\ {\mathrm{\ω}}_{\mathrm{xy}}& {\mathrm{\ω}}_{\mathrm{yy}}& {\mathrm{\ω}}_{\mathrm{zy}}\\ {\mathrm{\ω}}_{\mathrm{xz}}& {\mathrm{\ω}}_{\mathrm{yz}}& {\mathrm{\ω}}_{\mathrm{zz}}\end{array}\right]$

△ t is the sampling time.

Step 2.3: calculate ω _x, ω _y, ω _z

ω _x=(ω _xk-1+ω _xk)/2

ω _y=(ω _yk-1+ω _yk)/2

ω _z=(ω _zk-1+ω _zk)/2

Step 2.4: calculate C _ipk

$C_{\mathrm{ipk}}=C_{\mathrm{ipk}-1}\left[\begin{array}{ccc}1-({\mathrm{\ω}}_y^2+{\mathrm{\ω}}_z^2){\mathrm{\Δt}}^2/2& {-\mathrm{\ω}}_z\mathrm{\Δt}+{\mathrm{\ω}}_x{\mathrm{\ω}}_y{\mathrm{\Δt}}^2/2& {\mathrm{\ω}}_y\mathrm{\Δt}+{\mathrm{\ω}}_x{\mathrm{\ω}}_z{\mathrm{\Δt}}^2/2\\ {\mathrm{\ω}}_z\mathrm{\Δt}+{\mathrm{\ω}}_x{\mathrm{\ω}}_y{\mathrm{\Δt}}^2/2& 1-({\mathrm{\ω}}_x^2+{\mathrm{\ω}}_z^2){\mathrm{\Δt}}^2/2& {-\mathrm{\ω}}_x\mathrm{\Δt}+{\mathrm{\ω}}_y{\mathrm{\ω}}_z{\mathrm{\Δt}}^2/2\\ {-\mathrm{\ω}}_y\mathrm{\Δt}+{\mathrm{\ω}}_x{\mathrm{\ω}}_z{\mathrm{\Δt}}^2/2& {\mathrm{\ω}}_x\mathrm{\Δt}+{\mathrm{\ω}}_y{\mathrm{\ω}}_z{\mathrm{\Δt}}^2/2& 1-({\mathrm{\ω}}_y^2+{\mathrm{\ω}}_x^2){\mathrm{\Δt}}^2/2\end{array}\right]$

Step 2.5: calculate C _gik

Ω is earth rotation angular speed.

Step 2.6: calculate C _gpk

C _gpk=C _gik·C _ipk，

Step 2.7: calculating location r _gk

$f_k=\left[\begin{array}{c}{f}_{\mathrm{xk}}\\ f_{\mathrm{yk}}\\ f_{\mathrm{zk}}\end{array}\right]=\left(\begin{array}{c}{n}_{0x}\\ n_{0y}\\ n_{0z}\end{array}\right)+\left(\begin{array}{ccc}1+{\mathrm{\Δ}}_{\mathrm{xx}}& 0& 0\\ {\mathrm{\Δ}}_{\mathrm{yx}}& 1+{\mathrm{\Δ}}_{\mathrm{yy}}& 0\\ {\mathrm{\Δ}}_{\mathrm{zx}}& {\mathrm{\Δ}}_{\mathrm{zy}}& 1+{\mathrm{\Δ}}_{\mathrm{zz}}\end{array}\right)\left(\begin{array}{c}{b}_{\mathrm{xk}}{M}_x\\ b_{\mathrm{yk}}{M}_y\\ b_{\mathrm{zk}}{M}_z\end{array}\right)$

$V_{g0}^T=\left[\begin{array}{ccc}{V}_x\left(0\right)& V_y\left(0\right)& V_z\left(0\right)\end{array}\right]$

$r_{\mathrm{gk}}=r_{\mathrm{gk}-1}+\frac{1}{2}(V_{\mathrm{gk}}+V_{\mathrm{gk}-1})\mathrm{\Δt}$

Step 2.6 calculates

calculate with step 2.7 $r_{\mathrm{xgk}}^{\left(j\right)},r_{\mathrm{ygk}}^{\left(j\right)},r_{\mathrm{zgk}}^{\left(j\right)}(j=\mathrm{1,2},k=1,...,N^j)$

A kind of quick dynamic alignment method as above wherein, increases following step after step 2,

Step 3: intermediate variable calculates

Step 3.1: input message

This step, except the input message of preceding step and the result of calculating, also needs to input each position and revolves parameter certificate

Step 3.2: calculate:

With following formula, calculate

$r_{\mathrm{xg}}\left(t\right)=V_{x}t+{\mathrm{\α}}_z\frac{{\mathrm{gt}}^2}{2}+{\mathrm{\ω}}_{\mathrm{zg}}\frac{{\mathrm{gt}}^3}{6}-R_x{\mathrm{\θ}}_z\left(t\right);$

$r_{\mathrm{zg}}\left(t\right)=V_{z}t-{\mathrm{\α}}_x\frac{{\mathrm{gt}}^2}{2}-{\mathrm{\ω}}_{\mathrm{xg}}\frac{{\mathrm{gt}}^3}{6}+R_z{\mathrm{\θ}}_x\left(t\right);$

θ _y(t)＝θ _y0+ω _ygt.

Least square

Z _m×1＝H _m×nX _n×1

The least-squares estimation of X is: ${\hat{X}}_{n\×1}={\left(H_{n\×m}^T{H}_{m\×n}\right)}^{-1}{H}_{n\×m}^T{Z}_{m\×1}$

Wherein $Z_{m\×1}={\left[\begin{array}{cccc}{r}_{\mathrm{xg}}\left(t\right)& r_{\mathrm{yg}}\left(t\right)& r_{\mathrm{zg}}\left(t\right)& {\mathrm{\θ}}_y\left(t\right)\end{array}\right]}^T$

A kind of quick dynamic alignment method as above wherein, increases following step after step 3,

Step 4: aim at

With following formula, calculate:

${\widehat{\mathrm{\ω}}}_{0z}=\frac{1}{2}[({\mathrm{\ω}}_{\mathrm{xg}}^{\left(2\right)}-{\mathrm{\ω}}_{\mathrm{xg}}^{\left(1\right)})\sin K+({\mathrm{\ω}}_{\mathrm{zg}}^{\left(2\right)}-{\mathrm{\ω}}_{\mathrm{zg}}^{\left(1\right)})\cos K];{\widehat{\mathrm{\ω}}}_{0y}=-\frac{1}{2}({\mathrm{\ω}}_{\mathrm{yg}}^{\left(2\right)}+{\mathrm{\ω}}_{\mathrm{yg}}^{\left(1\right)}).$

The result calculating is net result.

The invention has the beneficial effects as follows: the present invention has at carrier can accurately estimate under the condition of angular oscillation that platform is three misalignments and three uncompensated platform drift (platform system) of relative Department of Geography, thereby has reached high-precision autoregistration object.

Embodiment

A dynamic alignment method, comprises the steps:

Step 1: coarse alignment

Step 1.1: input message

Need the information of input to comprise following three classes

(a) accelerometer data

$b_{\mathrm{xk}}^{\left(2\right)},b_{\mathrm{yk}}^{\left(2\right)},b_{\mathrm{zk}}^{\left(2\right)}(k=1,...,N^{\left(2\right)})$

Above-mentioned xyz refers to the direction of acceleration, and subscript k represents the data of k sampling instant, and the footmark in the upper right corner (2) refers to position two, N ⁽²⁾data altogether in the second place.

(b) revolve parameter certificate

J represents position, and the application has two positions, and the footmark in the Er， lower right corner, Yi He position, position 1,2,3 represents the data that different sensors provide, and needs input in each position, the zero hour revolve parameter certificate

The meaning of all footmarks and front identical, described T ₁refer to the finish time, T ₁/ 2 refer in the middle of constantly, need input in each position, constantly middle and finish time revolve parameter certificate.

(c) accelerometer parameter

n _0x,n _0y,n _0z,M _x,M _y,M _z,Δ _yx,Δ _zx,Δ _zy

Described n _0x, n _0y, n _0zfor accelerometer bias, M _x, M _y, M _zfor accelerometer calibration factor, Δ _yx, Δ _zx, Δ _zyfor accelerometer alignment error.

Above-mentioned accelerometer parameter all can directly obtain.

Step 1.2: calculate a _x, a _y, a _z

$a_x=\frac{1}{T}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}\left(b_{\mathrm{xk}}^{\left(2\right)}{M}_x\right)-n_{0x}$

$a_y=\frac{1}{T}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}(b_{\mathrm{yk}}^{\left(2\right)}{M}_y-b_{\mathrm{xk}}^{\left(2\right)}{M}_x{\mathrm{\Δ}}_{\mathrm{yx}})-n_{0y}$

$a_z=\frac{1}{T}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}(b_{\mathrm{zk}}^{\left(2\right)}{M}_z-b_{\mathrm{xk}}^{\left(2\right)}{M}_x{\mathrm{\Δ}}_{\mathrm{zx}}-b_{\mathrm{yk}}^{\left(2\right)}{M}_y{\mathrm{\Δ}}_{\mathrm{zy}})-n_{0z}$

In formula:

N _0x, n _0y, n _0zfor accelerometer bias; M _x, M _y, M _zfor accelerometer calibration factor;

Δ _yx, Δ _zx, Δ _zyfor accelerometer alignment error.

Step 1.3: calculate ω _x, ω _z

In formula:

for T ₁the angle in/2 moment.

Step 1.4: calculate

$C_{\mathrm{gp}}^{\left(2\right)}(T_1/2)=\left(\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right)$

In formula:

$c_{21}=\frac{a_x}{\sqrt{a_x^2+a_y^2+a_z^2}}$

$c_{22}=\frac{a_y}{\sqrt{a_x^2+a_y^2+a_z^2}}$

$c_{23}=\frac{a_z}{\sqrt{a_x^2+a_y^2+a_z^2}}$

c ₁₁=c ₂₂c ₃₃-c ₃₂c ₂₃

c ₁₂=c ₃₁c ₂₃-c ₂₁c ₃₃

c ₁₃=c ₂₁c ₃₂-c ₃₁c ₂₂

Wherein Ω is earth rotation angular speed.

Step 1.5: calculate

$C_{\mathrm{mg}}^{\left(2\right)}(T_1/2)=C_{\mathrm{mp}}^{\left(2\right)}(T_1/2)C_{\mathrm{pg}}^{\left(2\right)}(T_1/2)$

In formula

$C_{\mathrm{mp}}=\left(\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right)$ Here

extremely

for T ₁the angle in/2 moment.

Step 1.6: calculate

$C_{\mathrm{gp}}^{\left(j\right)}\left(0\right)={\left[C_{\mathrm{pm}}^{\left(j\right)}\left(0\right)C_{\mathrm{mg}}^{\left(2\right)}(T_1/2)\right]}^T(j=\mathrm{1,2})$

output data for this step

Step 2: navigation calculation

Step 2.1: input

The parameter that the parameter that this step needs has been inputted in step 1, also comprise

(a) platform drift and accelerometer parameter

ω _0x, ω _0y, ω _0zthe irrelevant drift of platform and acceleration, these three parameters are the intrinsic parameter of platform,

ω _xx, ω _xz, ω _yx, ω _yy, ω _zx, ω _zzplatform and acceleration associated drift, these six parameters are the input that sensor measurement obtains;

N _0x, n _0y, n _0z, M _x, M _y, M _zaccelerometer bias and calibration factor, these six parameters are the intrinsic parameter of accelerometer;

Each position

(

), this parameter is that step 1 calculates;

The linear velocity of pedestal

these three obtain for sensor measurement.

Step 2.2: calculate ω _xk-1, ω _xk, ω _yk-1, ω _yk, ω _zk-1, ω _zk

By following formula, calculate

C _ip0=C _gp（0)

$C_{\mathrm{ipk}}=C_{\mathrm{ipk}-1}\left[\begin{array}{ccc}1& {-\mathrm{\ω}}_{\mathrm{zk}-1}\mathrm{\Δt}& {\mathrm{\ω}}_{\mathrm{yk}-1}\mathrm{\Δt}\\ {\mathrm{\ω}}_{\mathrm{zk}-1}\mathrm{\Δt}& 1& {-\mathrm{\ω}}_{\mathrm{xk}-1}\mathrm{\Δt}\\ {-\mathrm{\ω}}_{\mathrm{yk}-1}\mathrm{\Δt}& {\mathrm{\ω}}_{\mathrm{xk}-1}\mathrm{\Δt}& 1\end{array}\right]$

${\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{xk}}\\ {\mathrm{\ω}}_{\mathrm{yk}}\\ {\mathrm{\ω}}_{\mathrm{zk}}\end{array}\right]}^T={\left[\begin{array}{c}{\mathrm{\ω}}_{0x}\\ {\mathrm{\ω}}_{0y}\\ {\mathrm{\ω}}_{0z}\end{array}\right]}^T+{\left[\begin{array}{c}{C}_{\mathrm{gp}21k}\\ C_{\mathrm{gp}22k}\\ C_{\mathrm{gp}23k}\end{array}\right]}^T\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{xx}}& {\mathrm{\ω}}_{\mathrm{yx}}& {\mathrm{\ω}}_{\mathrm{zx}}\\ {\mathrm{\ω}}_{\mathrm{xy}}& {\mathrm{\ω}}_{\mathrm{yy}}& {\mathrm{\ω}}_{\mathrm{zy}}\\ {\mathrm{\ω}}_{\mathrm{xz}}& {\mathrm{\ω}}_{\mathrm{yz}}& {\mathrm{\ω}}_{\mathrm{zz}}\end{array}\right]$

${\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{xk}-1}\\ {\mathrm{\ω}}_{\mathrm{yk}-1}\\ {\mathrm{\ω}}_{\mathrm{zk}-1}\end{array}\right]}^T={\left[\begin{array}{c}{\mathrm{\ω}}_{0x}\\ {\mathrm{\ω}}_{0y}\\ {\mathrm{\ω}}_{0z}\end{array}\right]}^T+{\left[\begin{array}{c}{C}_{\mathrm{gp}21k-1}\\ C_{\mathrm{gp}22k-1}\\ C_{\mathrm{gp}23k-1}\end{array}\right]}^T\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{xx}}& {\mathrm{\ω}}_{\mathrm{yx}}& {\mathrm{\ω}}_{\mathrm{zx}}\\ {\mathrm{\ω}}_{\mathrm{xy}}& {\mathrm{\ω}}_{\mathrm{yy}}& {\mathrm{\ω}}_{\mathrm{zy}}\\ {\mathrm{\ω}}_{\mathrm{xz}}& {\mathrm{\ω}}_{\mathrm{yz}}& {\mathrm{\ω}}_{\mathrm{zz}}\end{array}\right]$

△ t is the sampling time.

Step 2.3: calculate ω _x, ω _y, ω _z

ω _x=(ω _xk-1+ω _xk)/2

ω _y=(ω _yk-1+ω _yk)/2

ω _z=(ω _zk-1+ω _zk)/2

Step 2.4: calculate C _ipk

$C_{\mathrm{ipk}}=C_{\mathrm{ipk}-1}\left[\begin{array}{ccc}1-({\mathrm{\ω}}_y^2+{\mathrm{\ω}}_z^2){\mathrm{\Δt}}^2/2& {-\mathrm{\ω}}_z\mathrm{\Δt}+{\mathrm{\ω}}_x{\mathrm{\ω}}_y{\mathrm{\Δt}}^2/2& {\mathrm{\ω}}_y\mathrm{\Δt}+{\mathrm{\ω}}_x{\mathrm{\ω}}_z{\mathrm{\Δt}}^2/2\\ {\mathrm{\ω}}_z\mathrm{\Δt}+{\mathrm{\ω}}_x{\mathrm{\ω}}_y{\mathrm{\Δt}}^2/2& 1-({\mathrm{\ω}}_x^2+{\mathrm{\ω}}_z^2){\mathrm{\Δt}}^2/2& {-\mathrm{\ω}}_x\mathrm{\Δt}+{\mathrm{\ω}}_y{\mathrm{\ω}}_z{\mathrm{\Δt}}^2/2\\ {-\mathrm{\ω}}_y\mathrm{\Δt}+{\mathrm{\ω}}_x{\mathrm{\ω}}_z{\mathrm{\Δt}}^2/2& {\mathrm{\ω}}_x\mathrm{\Δt}+{\mathrm{\ω}}_y{\mathrm{\ω}}_z{\mathrm{\Δt}}^2/2& 1-({\mathrm{\ω}}_y^2+{\mathrm{\ω}}_x^2){\mathrm{\Δt}}^2/2\end{array}\right]$

Step 2.5: calculate C _gik

Ω is earth rotation angular speed.

Step 2.6: calculate C _gpk

C _gpk=C _gik·C _ipk，

Step 2.7: calculating location r _gk

$f_k=\left[\begin{array}{c}{f}_{\mathrm{xk}}\\ f_{\mathrm{yk}}\\ f_{\mathrm{zk}}\end{array}\right]=\left(\begin{array}{c}{n}_{0x}\\ n_{0y}\\ n_{0z}\end{array}\right)+\left(\begin{array}{ccc}1+{\mathrm{\Δ}}_{\mathrm{xx}}& 0& 0\\ {\mathrm{\Δ}}_{\mathrm{yx}}& 1+{\mathrm{\Δ}}_{\mathrm{yy}}& 0\\ {\mathrm{\Δ}}_{\mathrm{zx}}& {\mathrm{\Δ}}_{\mathrm{zy}}& 1+{\mathrm{\Δ}}_{\mathrm{zz}}\end{array}\right)\left(\begin{array}{c}{b}_{\mathrm{xk}}{M}_x\\ b_{\mathrm{yk}}{M}_y\\ b_{\mathrm{zk}}{M}_z\end{array}\right)$

$V_{g0}^T=\left[\begin{array}{ccc}{V}_x\left(0\right)& V_y\left(0\right)& V_z\left(0\right)\end{array}\right]$

$r_{\mathrm{gk}}=r_{\mathrm{gk}-1}+\frac{1}{2}(V_{\mathrm{gk}}+V_{\mathrm{gk}-1})\mathrm{\Δt}$

Step 2.6 calculates calculate with step 2.7 $r_{\mathrm{xgk}}^{\left(j\right)},r_{\mathrm{ygk}}^{\left(j\right)},r_{\mathrm{zgk}}^{\left(j\right)}(j=\mathrm{1,2},k=1,...,N^j)$

Step 3: intermediate variable calculates

Step 3.1: input message

This step, except the input message of preceding step and the result of calculating, also needs to input each position and revolves parameter certificate

Step 3.2: calculate:

With following formula, calculate

$r_{\mathrm{xg}}\left(t\right)=V_{x}t+{\mathrm{\α}}_z\frac{{\mathrm{gt}}^2}{2}+{\mathrm{\ω}}_{\mathrm{zg}}\frac{{\mathrm{gt}}^3}{6}-R_x{\mathrm{\θ}}_z\left(t\right);$

$r_{\mathrm{zg}}\left(t\right)=V_{z}t-{\mathrm{\α}}_x\frac{{\mathrm{gt}}^2}{2}-{\mathrm{\ω}}_{\mathrm{xg}}\frac{{\mathrm{gt}}^3}{6}+R_z{\mathrm{\θ}}_x\left(t\right);$

θ _y(t)＝θ _y0+ω _ygt.

Least square

Z _m×1=H _m×nX _n×1

The least-squares estimation of X is: ${\hat{X}}_{n\×1}={\left(H_{n\×m}^T{H}_{m\×n}\right)}^{-1}{H}_{n\×m}^T{Z}_{m\×1}$

Wherein $Z_{m\×1}={\left[\begin{array}{cccc}{r}_{\mathrm{xg}}\left(t\right)& r_{\mathrm{yg}}\left(t\right)& r_{\mathrm{zg}}\left(t\right)& {\mathrm{\θ}}_y\left(t\right)\end{array}\right]}^T$

Step 4: aim at

With following formula, calculate:

${\widehat{\mathrm{\ω}}}_{0z}=\frac{1}{2}[({\mathrm{\ω}}_{\mathrm{xg}}^{\left(2\right)}-{\mathrm{\ω}}_{\mathrm{xg}}^{\left(1\right)})\sin K+({\mathrm{\ω}}_{\mathrm{zg}}^{\left(2\right)}-{\mathrm{\ω}}_{\mathrm{zg}}^{\left(1\right)})\cos K];{\widehat{\mathrm{\ω}}}_{0y}=-\frac{1}{2}({\mathrm{\ω}}_{\mathrm{yg}}^{\left(2\right)}+{\mathrm{\ω}}_{\mathrm{yg}}^{\left(1\right)}).$

The result calculating is net result.

Claims (4)

1. a quick dynamic alignment method, is characterized in that, comprises the steps:

Step 1: coarse alignment

Step 1.1: input message

Need the information of input to comprise following three classes

(a) accelerometer data

Above-mentioned xyz refers to the direction of acceleration, and subscript k represents the data of k sampling instant, and the footmark in the upper right corner (2) refers to position two, N ⁽²⁾data altogether in the second place.

(b) revolve parameter certificate

J represents position, and the application has two positions, and the footmark in the Er， lower right corner, Yi He position, position 1,2,3 represents the data that different sensors provide, and needs input in each position, the zero hour revolve parameter certificate

The meaning of all footmarks and front identical, described T ₁refer to the finish time, T ₁/ 2 refer in the middle of constantly, need input in each position, constantly middle and finish time revolve parameter certificate.

(c) accelerometer parameter

n _0x,n _0y,n _0z,M _x,M _y,M _z,Δ _yx,Δ _zx,Δ _zy

Described n _0x, n _0y, n _0zfor accelerometer bias, M _x, M _y, M _zfor accelerometer calibration factor, Δ _yx, Δ _zx, Δ _zyfor accelerometer alignment error.

Above-mentioned accelerometer parameter all can directly obtain.

Step 1.2: calculate a _x, a _y, a _z

In formula:

N _0x, n _0y, n _0zfor accelerometer bias; M _x, M _y, M _zfor accelerometer calibration factor;

Δ _yx, Δ _zx, Δ _zyfor accelerometer alignment error.

Step 1.3: calculate ω _x, ω _z

In formula:

for T ₁the angle in/2 moment.

Step 1.4: calculate

In formula:

c ₁₁=c ₂₂c ₃₃-c ₃₂c ₂₃

c ₁₂=c ₃₁c ₂₃-c ₂₁c ₃₃

c ₁₃=c ₂₁c ₃₂-c ₃₁c ₂₂

Wherein Ω is earth rotation angular speed.

Step 1.5: calculate

In formula

here

extremely

for T ₁the angle in/2 moment.

Step 1.6: calculate

output data for this step.

2. a kind of quick dynamic alignment method as claimed in claim 1, is characterized in that: after step 1, increase following step,

Step 2: navigation calculation

Step 2.1: input

The parameter that the parameter that this step needs has been inputted in step 1, also comprise

(a) platform drift and accelerometer parameter

ω _0x, ω _0y, ω _0zthe irrelevant drift of platform and acceleration, these three parameters are the intrinsic parameter of platform,

ω _xx, ω _xz, ω _yx, ω _yy, ω _zx, ω _zzplatform and acceleration associated drift, these six parameters are the input that sensor measurement obtains;

N _0x, n _0y, n _0z, M _x, M _y, M _zaccelerometer bias and calibration factor, these six parameters are the intrinsic parameter of accelerometer;

Each position

(

), this parameter is that step 1 calculates;

The linear velocity of pedestal

these three obtain for sensor measurement.

Step 2.2: calculate ω _xk-1, ω _xk, ω _yk-1, ω _yk, ω _zk-1, ω _zk

By following formula, calculate

C _ip0=C _gp（0)

Δ t is the sampling time.

Step 2.3: calculate ω _x, ω _y, ω _z

ω _x=(ω _xk-1+ω _xk)/2

ω _y=(ω _yk-1+ω _yk)/2

ω _z=(ω _zk-1+ω _zk)/2

Step 2.4: calculate C _ipk

Step 2.5: calculate C _gik

Ω is earth rotation angular speed.

Step 2.6: calculate C _gpk

C _gpk=C _gik·C _ipk，

Step 2.7: calculating location r _gk

Step 2.6 calculates

calculate with step 2.7

.

3. a kind of quick dynamic alignment method as claimed in claim 2, is characterized in that: after step 2, increase following step,

Step 3: intermediate variable calculates

Step 3.1: input message

This step, except the input message of preceding step and the result of calculating, also needs to input each position and revolves parameter certificate

Step 3.2: calculate:

With following formula, calculate

θ _y(t)＝θ _y0+ω _ygt.

Least square

Z _m×1=H _m×nX _n×1

The least-squares estimation of X is:

Wherein

。

4. a kind of quick dynamic alignment method as claimed in claim 3, is characterized in that: after step 3, increase following step,

Step 4: aim at

With following formula, calculate:

The result calculating is net result.