[发明专利]断调平状态下平台惯导对准方法

摘要

本发明的目的在于提供断调平状态下平台惯导对准方法，把子惯导进入断调平状态初始时刻的地球系凝固于惯性空间中，将子惯导进入断调平状态初始时刻的主惯导平台系凝固于惯性空间中，将平台台失准角分成两部分，一部分为主惯导平台系与主惯导平台惯性系之间的夹角，另一部分即是子惯导平台系与主惯导平台惯性系之间的夹角，通过实时更新地理系相对于地心惯性系的余弦矩阵，在地心惯性系上对主、子惯导建立速度匹配误差模型，通过卡尔曼滤波得到子惯导平台与主惯导平台惯性系之间的夹角，再利用上述余弦矩阵，得到主、子惯导平台之间的失准角。本发明解决了传统速度匹配方法无法对Z轴失准角进行估计的问题，提高了Z轴方向上失准角的可观测度。

主权项

断调平状态下平台惯导对准方法，其特征是：(1)将子惯导进入断调平状态初始时刻的子惯导平台系凝固于惯性空间中，定义为子惯导平台惯性系p₀，之后子惯导平台系为s，此时主惯导平台依旧工作在地理系n；(2)将子惯导进入断调平状态初始时刻的主惯导平台系凝固于惯性空间中，定义为主惯导平台惯性系m；(3)把子惯导进入断调平状态初始时刻的地球系凝固于惯性空间中，定义为地心惯性系i，把主惯导在地理系n上所测比力fⁿ投影到地心惯性系i上得到即为主惯导所测比力fⁿ在地心惯性系i的投影，为地理系n到的地心惯性系i的方向余弦矩阵；对于子惯导，定义子惯导在地心惯性系i所测比力f^j为f^s为子惯导在子惯导平台系s上所测比力，为地理系m到的地心惯性系i的方向余弦矩阵，然后在i系上建立速度误差微分方程与失准角微分方程；(4)建立i系上的速度方程：${\stackrel{\·}{v}}_i^i=f^i-g^i,$为主惯导的速度微分在地心惯性系i下的投影，且gⁱ为地理系下重力加速度矢量gⁿ在平台惯性系下投影则对子惯导也有：${\stackrel{\·}{v}}_j^j=f^j-g^j,$为子惯导的速度微分在地心惯性i系下的投影，对${\stackrel{\·}{v}}_i^i=f^i-g^i$以及${\stackrel{\·}{v}}_j^j=f^j-g^j$两式相减可得$\mathrm{\δ}{\stackrel{\·}{v}}_i^i=C_m^i{f}^s\×{\mathrm{\φ}}_i+C_m^i\▿,$即为速度误差微分方程，为子惯导与主惯导的速度微分之差，为子惯导的加速度计零偏；φ_i为子惯导平台系与主惯导平台惯性系之间夹角，不考虑挠曲变形，φ_i将只受子惯导的陀螺漂移影响：即为失准角微分方程；(5)利用步骤(4)得到的速度误差微分方程与失准角微分方程，建立速度匹配状态观测方程：$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_i^i=C_m^i{f}^s\×{\mathrm{\φ}}_i+C_m^i\▿\\ {\stackrel{\·}{\mathrm{\φ}}}_i=\mathrm{\ϵ}\\ \stackrel{\·}{\▿}=0\\ \stackrel{\·}{\mathrm{\ϵ}}=0\end{array}\right.$其中滤波模型为$\left\{\begin{array}{c}\stackrel{\·}{X}=\mathrm{AX}+\mathrm{BW}\\ Z=\mathrm{HX}+V\end{array}\right.$选取状态量为$X={\left[\begin{array}{cccccccccccc}{\mathrm{\δv}}_{\mathrm{ix}}^i& {\mathrm{\δv}}_{\mathrm{iy}}^i& {\mathrm{\δv}}_{\mathrm{iz}}^i& {\mathrm{\φ}}_{\mathrm{ix}}& {\mathrm{\φ}}_{\mathrm{iy}}& {\mathrm{\φ}}_{\mathrm{iz}}& {\▿}_x& {\▿}_y& {\▿}_z& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T$为子惯导与主惯导的速度微分之差在i系xyz三个方向下的投影，φ_ix,φ_iy,φ_iz为φi在i系xyz三个方向下的投影，为子惯导的加速度计零偏在i系xyz三个方向下的投影，ε_x,ε_y,ε_z为子惯导的陀螺漂移在i系xyz三个方向下的投影，则有：$A=\left[\begin{array}{cccc}{0}_{3\×3}& C_m^i[f^s\×]& C_m^i& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]B=\left[\begin{array}{ccc}{C}_m^i& 0_{3\×3}& 0_{3\×6}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×6}\end{array}\right]$$H=\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×9}\end{array}\right][f^s\×]=\left[\begin{array}{ccc}0& -f_z^s& f_y^s\\ f_z^s& 0& -f_x^s\\ {-f}_y^s& f_x^s& 0\end{array}\right]$$W={\left[\begin{array}{cccccccccccc}{w}_{\mathrm{ax}}& w_{\mathrm{ay}}& w_{\mathrm{az}}& w_{\mathrm{\ϵx}}& w_{\mathrm{\ϵy}}& w_{\mathrm{az}}& 0& 0& 0& 0& 0& 0\end{array}\right]}^T$w_ax、w_ay、w_az与w_εx,w_εy,w_εz分别为对应加速度计以及陀螺上的系统噪声。观测量Z为子惯导与主惯导在地心惯性系下速度的差值，V为量测噪声，且为地理系m到的地心惯性系i的方向余弦矩阵；(6)用步骤(5)的状态观测方程进行卡尔曼滤波，选取状态初值X₀＝0_12×1，初始均方误差阵为对角阵P₀，卡尔曼基本方程如下：状态一步预测${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$一步预测均方误差阵$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$滤波增益矩阵$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}$状态估计${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}]$估计均方误差阵$P_k=[I-K_k{H}_k]P_{k,k-1}{[I-K_k{H}_k]}^T+K_k{R}_k{K}_k^T$之后再返回状态一步预测，继续迭代，直到设定时间结束为止；其中为第k时刻系统状态估计，为根据k‑1时刻的状态估计对k时刻状态进行预测得到的状态，Φ_k,k‑1为k‑1时刻到k时刻的状态转移矩阵，P_k‑1为第k‑1时刻的均方误差阵，P_k,k‑1为为根据k‑1时刻的均方误差阵对k时刻均方误差阵进行预测得到的矩阵，K_k为第k时刻的滤波增益阵，Z_k为第k时刻的量测值，H_k为第k时刻的量测矩阵，I为单位阵，Γ_k‑1为第k‑1时刻的系统噪声驱动矩阵，R_k为第k时刻的观测噪声矩阵，Q_k‑1为第k‑1时刻的系统噪声矩阵；(7)利用步骤(6)得到的状态估计得到φ_ix,φ_iy,φ_iz，然后构建$C_m^s=I-[{\mathrm{\φ}}_i\×],$即$C_m^s=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_{\mathrm{iz}}& -{\mathrm{\φ}}_{\mathrm{iy}}\\ -{\mathrm{\φ}}_{\mathrm{iz}}& 1& {\mathrm{\φ}}_{\mathrm{iz}}\\ {\mathrm{\φ}}_{\mathrm{iy}}& -{\mathrm{\φ}}_{\mathrm{ix}}& 1\end{array}\right]$再通过$C_n^s=C_m^s{C}_n^m,$得到$[\mathrm{\φ}\×]=I-C_n^s,$且有$[\mathrm{\φ}\×]=\left[\begin{array}{ccc}0& -{\mathrm{\φ}}_z& {\mathrm{\φ}}_y\\ {\mathrm{\φ}}_z& 0& -{\mathrm{\φ}}_x\\ -{\mathrm{\φ}}_y& {\mathrm{\φ}}_x& 0\end{array}\right];$(8)利用步骤(7)得到的[φ×]值，得到子惯导此时的真实姿态，即可调整子惯导的物理平台，经过子惯导物理平台x,y,z三轴分别转动φ_x,φ_y,φ_z之后，即可使得子惯导的平台系与主惯导的平台系保持姿态一致。