CN103557873B - A kind of dynamic alignment method fast - Google Patents

Abstract

The invention belongs to alignment methods, be specifically related to a kind of dynamic alignment method fast. It comprises: step 1: coarse alignment; Step 1.1: input message; Step 1.2: calculate a_x、a_y、a_z; Step 1.3: calculate ω_x、ω_zStep 1.4: calculateStep 1.5: calculate Step 1.6: calculate

Description

A kind of dynamic alignment method fast

Technical field

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

Background technology

Platform Inertial Navigation System, externally exporting before navigation information, must be carried 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. But under swaying base condition, due to the angular oscillation of carrier, make inertial platform record gravity and earth rotation angular speed all disturbs, have a strong impact on alignment precision. Disturb in order to eliminate these, 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 dynamic alignment method is fast 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 (2) in the upper right corner refers to position two, N⁽²⁾Data altogether in the second place.

(b) revolve parameter certificate

J represents position, and the application has two positions, position one and position two, the footmark 1,2,3 in the lower right corner 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 the moment, need input in each position, middle moment and the 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

ExtremelyFor 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})$

For the output data of this step.

The quick dynamic alignment method of one 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 pedestalThese three obtain for sensor measurement.

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

Calculate by following formula

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 calculatesCalculate 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)$

The quick dynamic alignment method of one 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:

Calculate with following formula

$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$

The quick dynamic alignment method of one as above wherein, increases following step after step 3,

Step 4: aim at

Calculate with following formula:

${\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 final 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.

Detailed description of the invention

A kind of dynamic alignment method fast, 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 (2) in the upper right corner refers to position two, N⁽²⁾Data altogether in the second place.

(b) revolve parameter certificate

J represents position, and the application has two positions, position one and position two, the footmark 1,2,3 in the lower right corner 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 the moment, need input in each position, middle moment and the 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

ExtremelyFor 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})$

For the output data of 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 pedestalThese three obtain for sensor measurement.

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

Calculate by following formula

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 calculatesCalculate 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:

Calculate with following formula

$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

Calculate with following formula:

${\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 final result.

Claims (1)

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 (2) in the upper right corner refers to position two, N⁽²⁾Data altogether in the second place,

(b) revolve parameter certificate

J represents position, and the application has two positions, position one and position two, the footmark 1,2,3 in the lower right corner 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 the moment, need input in each position, middle moment and the 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

ExtremelyFor T₁The angle in/2 moment,

Step 1.6: calculate

For the output data of 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 pedestalThese three for sensor measurement obtains,

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

Calculate by following formula

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 calculatesCalculate with step 2.7

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:

Calculate with following formula

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

Least square

Z_m×1＝H_m×nX_n×1

The least-squares estimation of X is:

Wherein Z_m×1＝[r_xg(t)r_yg(t)r_zg(t)θ_y(t)]^T

Step 4: aim at

Calculate with following formula:

The result calculating is final result.