CN103557873B - 一种快速动态对准方法 - Google Patents

Abstract

本发明属于对准方法，具体涉及一种快速动态对准方法。它包括：步骤一：粗对准；步骤1.1：输入信息；步骤1.2：计算a_x、a_y、a_z；步骤1.3：计算ω_x、ω_z步骤1.4：计算步骤1.5：计算 步骤1.6：计算

Description

一种快速动态对准方法

技术领域

本发明属于对准方法，具体涉及一种快速动态对准方法。

背景技术

平台惯导***在能对外输出导航信息前，必须进行初始对准。在静基座条件下，为了完成初始对准，需要利用重力和地球自转角速率信息，使平台惯导***中平台台体在航向上工作在一个或多个不同位置，从而得出三个陀螺的漂移和平台系相对地理系的三个失准角。然而在晃动基座条件下，由于载体的角振动，使得惯性平台测得重力和地球自转角速率都有较大干扰，严重影响了对准精度。为了消除这些干扰，需要采用新的滤波方法完成晃动基座下的平台惯导自对准。

发明内容

本发明的目的是针对现有技术的缺陷，提供一种快速动态对准方法。

本发明是这样实现的：1.一种快速动态对准方法，其特征在于，包括下述步骤：

步骤一：粗对准

步骤1.1：输入信息

需要输入的信息包括下述三类

（a）加速度计数据

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

上述的xyz是指加速度的方向，下标k表示k采样时刻的数据，右上角的角标（2）是指位置二，N⁽²⁾是在第二位置中总共的数据。

（b）旋变数据

j表示位置，本申请共有两个位置，位置一和位置二，右下角的角标1、2、3表示不同传感器给出的数据，即需要输入在每个位置，开始时刻的旋变数据

所有角标的意义与前相同，所述的T₁是指结束时刻，T₁/2是指中间时刻，即需要输入在每个位置，中间时刻和结束时刻的旋变数据。

（c）加速度计参数

n_0x,n_0y,n_0z,M_x,M_y,M_z,△_yx,△_zx,△_zy

所述的n_0x,n_0y,n_0z为加速度计零偏，M_x,M_y,M_z为加速度计刻度系数，△_yx,△_zx,△_zy为加速度计安装误差。

上述加速度计参数均是可以直接得到的。

步骤1.2：计算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}$

式中：

n_0x,n_0y,n_0z为加速度计零偏；M_x,M_y,M_z为加速度计刻度系数；

Δ_yx,Δ_zx,Δ_zy为加速度计安装误差。

步骤1.3：计算ω_x、ω_z

式中：

为T₁/2时刻的角度。

步骤1.4：计算

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

式中：

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

其中Ω为地球自转角速率。

步骤1.5：计算

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

式中

$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)$ 这里的

至为T₁/2时刻的角度。

步骤1.6：计算

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

为本步骤的输出数据。

如上所述的一种快速动态对准方法，其中，在步骤一后增加下述步骤，

步骤二：导航解算

步骤2.1：输入

本步骤需要的参数除了步骤一中已经输入的参数外，还包括

（a）平台漂移和加速度计参数

ω_0x,ω_0y,ω_0z平台与加速度无关漂移，该三个参数为平台固有参数，

ω_xx,ω_xz,ω_yx,ω_yy,ω_zx,ω_zz平台与加速度相关漂移，该六个参数为传感器测量得到的输入；

n_0x,n_0y,n_0z,M_x,M_y,M_z加速度计零偏和刻度系数，该六个参数为加速度计固有参数；

每个位置(即)，该参数为步骤一计算得到；

基座的线速度该三个为传感器测量得到。

步骤2.2：计算ω_xk-1、ω_xk、ω_yk-1、ω_yk、ω_zk-1、ω_zk

通过下述公式进行计算

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为采样时间。

步骤2.3：计算ω_x、ω_y、ω_z

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

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

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

步骤2.4：计算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]$

步骤2.5：计算C_gik

Ω是地球自转角速率。

步骤2.6：计算C_gpk

C_gpk=C_gik·C_ipk，

步骤2.7：计算位置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}$

步骤2.6计算得到的和步骤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)$

如上所述的一种快速动态对准方法，其中，在步骤二后增加下述步骤，

步骤三：中间变量计算

步骤3.1：输入信息

本步骤除了前面步骤的输入信息和计算的结果外，还需要输入每个位置旋变数据

步骤3.2：计算：

用下述公式进行计算

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

最小二乘

Z_m×1＝H_m×nX_n×1

则X的最小二乘估计为： ${\hat{X}}_{n\×1}={\left(H_{n\×m}^T{H}_{m\×n}\right)}^{-1}{H}_{n\×m}^T{Z}_{m\×1}$

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

如上所述的一种快速动态对准方法，其中，在步骤三后增加下述步骤，

步骤四：对准

用下述公式进行计算：

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

计算得到的结果即为最终结果。

本发明的有益效果是：本发明在载体有角振动的条件下能准确估算平台系相对地理系的三个失准角以及三个未补偿的平台漂移（平台系），从而达到了高精度的自对准目的。

具体实施方式

一种快速动态对准方法，包括下述步骤：

步骤一：粗对准

步骤1.1：输入信息

需要输入的信息包括下述三类

（a）加速度计数据

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

上述的xyz是指加速度的方向，下标k表示k采样时刻的数据，右上角的角标（2）是指位置二，N⁽²⁾是在第二位置中总共的数据。

（b）旋变数据

j表示位置，本申请共有两个位置，位置一和位置二，右下角的角标1、2、3表示不同传感器给出的数据，即需要输入在每个位置，开始时刻的旋变数据

所有角标的意义与前相同，所述的T₁是指结束时刻，T₁/2是指中间时刻，即需要输入在每个位置，中间时刻和结束时刻的旋变数据。

（c）加速度计参数

n_0x,n_0y,n_0z,M_x,M_y,M_z,Δ_yx,Δ_zx,Δ_zy

所述的n_0x,n_0y,n_0z为加速度计零偏，M_x,M_y,M_z为加速度计刻度系数，Δ_yx,Δ_zx,Δ_zy为加速度计安装误差。

上述加速度计参数均是可以直接得到的。

步骤1.2：计算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}$

式中：

n_0x,n_0y,n_0z为加速度计零偏；M_x,M_y,M_z为加速度计刻度系数；

Δ_yx,Δ_zx,Δ_zy为加速度计安装误差。

步骤1.3：计算ω_x、ω_z

式中：

为T₁/2时刻的角度。

步骤1.4：计算

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

式中：

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

其中Ω为地球自转角速率。

步骤1.5：计算

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

式中

$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)$ 这里的

至为T₁/2时刻的角度。

步骤1.6：计算

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

为本步骤的输出数据

步骤二：导航解算

步骤2.1：输入

本步骤需要的参数除了步骤一中已经输入的参数外，还包括

（a）平台漂移和加速度计参数

ω_0x,ω_0y,ω_0z平台与加速度无关漂移，该三个参数为平台固有参数，

ω_xx,ω_xz,ω_yx,ω_yy,ω_zx,ω_zz平台与加速度相关漂移，该六个参数为传感器测量得到的输入；

n_0x,n_0y,n_0z,M_x,M_y,M_z加速度计零偏和刻度系数，该六个参数为加速度计固有参数；

每个位置(即)，该参数为步骤一计算得到；

基座的线速度该三个为传感器测量得到。

步骤2.2：计算ω_xk-1、ω_xk、ω_yk-1、ω_yk、ω_zk-1、ω_zk

通过下述公式进行计算

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为采样时间。

步骤2.3：计算ω_x、ω_y、ω_z

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

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

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

步骤2.4：计算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]$

步骤2.5：计算C_gik

Ω是地球自转角速率。

步骤2.6：计算C_gpk

C_gpk=C_gik·C_ipk，

步骤2.7：计算位置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}$

步骤2.6计算得到的和步骤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)$

步骤三：中间变量计算

步骤3.1：输入信息

本步骤除了前面步骤的输入信息和计算的结果外，还需要输入每个位置旋变数据

步骤3.2：计算：

用下述公式进行计算

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

最小二乘

Z_m×1=H_m×nX_n×1

则X的最小二乘估计为： ${\hat{X}}_{n\×1}={\left(H_{n\×m}^T{H}_{m\×n}\right)}^{-1}{H}_{n\×m}^T{Z}_{m\×1}$

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

步骤四：对准

用下述公式进行计算：

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

计算得到的结果即为最终结果。

Claims (1)

1.一种快速动态对准方法，其特征在于，包括下述步骤：

步骤一：粗对准

步骤1.1：输入信息

需要输入的信息包括下述三类

(a)加速度计数据

上述的xyz是指加速度的方向，下标k表示k采样时刻的数据，右上角的角标(2)是指位置二，N⁽²⁾是在第二位置中总共的数据，

(b)旋变数据

j表示位置，本申请共有两个位置，位置一和位置二，右下角的角标1、2、3表示不同传感器给出的数据，即需要输入在每个位置，开始时刻的旋变数据

所有角标的意义与前相同，所述的T₁是指结束时刻，T₁/2是指中间时刻，即需要输入在每个位置，中间时刻和结束时刻的旋变数据，

(c)加速度计参数

n_0x,n_0y,n_0z,M_x,M_y,M_z,Δ_yx,Δ_zx,Δ_zy

所述的n_0x,n_0y,n_0z为加速度计零偏，M_x,M_y,M_z为加速度计刻度系数，Δ_yx,Δ_zx,Δ_zy为加速度计安装误差，

上述加速度计参数均是可以直接得到的，

步骤1.2：计算a_x、a_y、a_z

式中：

n_0x,n_0y,n_0z为加速度计零偏；M_x,M_y,M_z为加速度计刻度系数；

Δ_yx,Δ_zx,Δ_zy为加速度计安装误差，

步骤1.3：计算ω_x、ω_z

式中：

为T₁/2时刻的角度，

步骤1.4：计算

式中：

c₁₁＝c₂₂c₃₃-c₃₂c₂₃

c₁₂＝c₃₁c₂₃-c₂₁c₃₃

c₁₃＝c₂₁c₃₂-c₃₁c₂₂

其中Ω为地球自转角速率，

步骤1.5：计算

式中

这里的

至为T₁/2时刻的角度，

步骤1.6：计算

为本步骤的输出数据；

步骤二：导航解算

步骤2.1：输入

本步骤需要的参数除了步骤一中已经输入的参数外，还包括

(a)平台漂移和加速度计参数

ω_0x,ω_0y,ω_0z平台与加速度无关漂移，该三个参数为平台固有参数，

ω_xx,ω_xz,ω_yx,ω_yy,ω_zx,ω_zz平台与加速度相关漂移，该六个参数为传感器测量得到的输入；

n_0x,n_0y,n_0z,M_x,M_y,M_z加速度计零偏和刻度系数，该六个参数为加速度计固有参数；

每个位置(即)，该参数为步骤一计算得到；

基座的线速度该三个为传感器测量得到，

步骤2.2：计算ω_xk-1、ω_xk、ω_yk-1、ω_yk、ω_zk-1、ω_zk

通过下述公式进行计算

C_ip0＝C_gp(0)

Δt为采样时间，

步骤2.3：计算ω_x、ω_y、ω_z

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

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

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

步骤2.4：计算C_ipk

步骤2.5：计算C_gik

Ω是地球自转角速率，

步骤2.6：计算C_gpk

C_gpk=C_gik·C_ipk，

步骤2.7：计算位置r_gk

步骤2.6计算得到的和步骤2.7计算得到的

步骤三：中间变量计算

步骤3.1：输入信息

本步骤除了前面步骤的输入信息和计算的结果外，还需要输入每个位置旋变数据

步骤3.2：计算：

用下述公式进行计算

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

最小二乘

Z_m×1＝H_m×nX_n×1

则X的最小二乘估计为：

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

步骤四：对准

用下述公式进行计算：

计算得到的结果即为最终结果。