一种带双轴转位机构捷联惯导的三位置自对准方法与流程

技术特征：

1.一种带双轴转位机构捷联惯导的三位置自对准方法，其特征在于，包括以下步骤：

步骤1，建立改进的卡尔曼滤波模型：

以东北天坐标系作为导航坐标系，其中导航坐标系用n表示、机体坐标系用b表示、惯性坐标系用i表示；ω_ie中的下标表示地球坐标系相对于惯性坐标系的旋转角速度，ε^b表示等效陀螺漂移ε在b系内的投影，▽^b表示等效加速度计的零偏▽在b系内的投影；速度误差向量δVⁿ、姿态误差向量φ、导航坐标系下的陀螺常值漂移εⁿ和加速度计零偏▽ⁿ，其形式如下所示：

${\mathrm{\δV}}^n=\left[\begin{array}{c}\mathrm{\δ}{V}_E\\ {\mathrm{\δV}}_N\\ {\mathrm{\δV}}_U\end{array}\right],\mathrm{\φ}=\left[\begin{array}{c}{\mathrm{\φ}}_E\\ {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U\end{array}\right],{\▿}^n=\left[\begin{array}{c}{\▿}_E\\ {\▿}_N\\ {\▿}_U\end{array}\right],{\mathrm{\ϵ}}^n=\left[\begin{array}{c}{\mathrm{\ϵ}}_E\\ {\mathrm{\ϵ}}_N\\ {\mathrm{\ϵ}}_U\end{array}\right]$

在自对准过程中，载体无移动，捷联惯导系统的速度误差方程为：

$\mathrm{\δ}{\stackrel{\·}{V}}^n=F_{vv}\·{\mathrm{\δV}}^n+F_{v\mathrm{\φ}}\·\mathrm{\φ}+{\▿}^n---\left(1\right)$

其中，F_vφ＝(fⁿ×)，“×”表示叉乘运算；

捷联惯导系统的姿态误差方程为：

$\stackrel{\·}{\mathrm{\φ}}=F_{\mathrm{\φ}v}\·{\mathrm{\δV}}^n+F_{\mathrm{\φ}\mathrm{\φ}}\·\mathrm{\φ}-{\mathrm{\ϵ}}^n---\left(2\right)$

其中，R_M、R_N分别为经纬圈和卯酉圈半径，L、h分别为纬度和高度；

将等效陀螺的漂移ε^b和等效加速度计零偏▽^b视为随机常值误差，选取状态变量为：X＝[δVⁿ φ ▽ⁿ εⁿ]^Τ，同时以速度误差δVⁿ作为外界量测，则系统模型表示如下：

$\left\{\begin{array}{c}\stackrel{\·}{X}=F\·X\\ Z=H\·X+V\end{array}\right.---\left(3\right)$

其中H＝[I 0 0 0]，I为3阶单位矩阵，0为3阶零矩阵；V为由晃动等干扰引起的量测噪声，可以近似看作白噪声序列，假设其方差为R；

步骤2，可观测性分析：

依据连续时间线性定常系统可观测性分析相关理论，在单一位置对式(3)所表示的系统进行可观测性分析，构造可观测性矩阵Q＝[H^Τ (HF)^Τ … (HF¹¹)^Τ]^Τ，令Q₁＝[H^Τ (HF)^Τ… (HF⁴)^Τ]^Τ；

$Q_1=\left[\begin{array}{c}H\\ HF\\ {\mathrm{HF}}^2\\ {\mathrm{HF}}^3\\ {\mathrm{HF}}^4\end{array}\right]=\left[\begin{array}{cccc}I& 0& 0& 0\\ F_{vv}& F_{v\mathrm{\φ}}& I& 0\\ A_1& A_2& F_{vv}& -F_{v\mathrm{\φ}}\\ B_1& B_2& A_1& -A_2\\ C_1& C_2& B_1& -B_2\end{array}\right]---\left(4\right)$

其中

令其中N₂₁＝-F_vv，N₃₁＝-F_φv，

对式(4)左乘矩阵N进行行变换，得：

${\mathrm{NQ}}_1=\left[\begin{array}{c}H\\ N_{21}H+HF\\ N_{31}H+N_{32}HF+N_{34}{\mathrm{HF}}^3\\ N_{42}HF+{\mathrm{HF}}^2+N_{44}{\mathrm{HF}}^3\\ N_{52}HF+N_{54}{\mathrm{HF}}^3+{\mathrm{HF}}^4\end{array}\right]=\left[\begin{array}{cccc}I& 0& 0& 0\\ 0& F_{v\mathrm{\φ}}& I& 0\\ 0& F_{\mathrm{\φ}\mathrm{\φ}}& 0& -I\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]---\left(5\right)$

由式N₅₂HF+N₅₄HF³+HF⁴＝0可得，HF⁴由HF和HF³线性表示，因此HF^k均由HF和HF³线性表示，其中k＝4,5…11；因此rank(Q)＝rank(Q₁)＝rank(NQ₁)；

由式(5)可得，rank(NQ₁)＝9，因此得出结论：在任一固定位置，式(3)表示的系统可观测矩阵的秩为9，不满足完全可观测的条件；但是存在可观测组合，现加以分析：

记则Y＝Q₁X，令其中

$K_{11}=\left[\begin{array}{ccc}0& 0& 1\\ 0& \frac{1}{g}& 0\\ -\frac{1}{g}& 0& 0\end{array}\right],K_{21}=\left[\begin{array}{ccc}-\frac{\tan L}{g}& 0& 0\\ 0& -\frac{{\mathrm{\ω}}_{\mathrm{ie}}\sin L}{g}& 0\\ 0& \frac{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}{g}& 0\end{array}\right],K_{22}=\left[\begin{array}{ccc}-\frac{1}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}& 0& 0\\ 0& -1& 0\\ 0& 0& -1\end{array}\right]$

${\mathrm{MNQ}}_1=\left[\begin{array}{c}P\\ 0_{6\×12}\end{array}\right]---\left(6\right)$

其中

$MNY={\mathrm{MNQ}}_{1}X=\left[\begin{array}{c}P\\ 0_{6\×12}\end{array}\right]X---\left(7\right)$

则式(3)表示的系统的可观测组合状态如下：

$PX=\left[\begin{array}{c}{{\mathrm{\δV}}^n}_{3\×1}\\ {\▿}_U\\ {\mathrm{\φ}}_E+\frac{1}{g}{\▿}_N\\ {\mathrm{\φ}}_N-\frac{1}{g}{\▿}_E\\ {\mathrm{\φ}}_U+\frac{1}{{\mathrm{\ω}}_{ie}\cos L}{\mathrm{\ϵ}}_E-\frac{\tan L}{g}{\▿}_E\\ {\mathrm{\ϵ}}_N-\frac{{\mathrm{\ω}}_{ie}\sin L}{g}{\▿}_N\\ {\mathrm{\ϵ}}_U+\frac{{\mathrm{\ω}}_{ie}\cos L}{g}{\▿}_N\end{array}\right]---\left(8\right)$

步骤3，可观测组合的估计收敛速度分析：

由式(7)对式(8)表示的可观测组合的收敛速度进行分析；根据式(7)可知，MNY的前9阶对应可观测组合的量测，因此，仅需对MNY矩阵的前9行进行分析，过程如下：

[MN]_1:9Y＝[MNQ₁]_1:9X＝PX (9)

式中，下标1:9表示矩阵的前9行；

$\begin{array}{c}{\[MN\]}_{1:9}={\left(\left[\begin{array}{ccccc}I& 0& 0& 0& 0\\ 0& K_{11}& 0& 0& 0\\ 0& K_{21}& K_{22}& 0& 0\\ 0& 0& 0& I& 0\\ 0& 0& 0& 0& I\end{array}\right]\·\left[\begin{array}{ccccc}I& 0& 0& 0& 0\\ N_{21}& I& 0& 0& 0\\ N_{31}& N_{32}& 0& N_{34}& 0\\ 0& N_{42}& I& N_{44}& 0\\ 0& N_{52}& 0& N_{54}& I\end{array}\right]\right)}_{1:9}=\\ \left[\begin{array}{ccccc}I& 0& 0& 0& 0\\ K_{11}{N}_{21}& K_{11}& 0& 0& 0\\ K_{21}{N}_{21}+K_{22}{N}_{31}& K_{21}+K_{22}{N}_{32}& 0& K_{22}{N}_{34}& 0\end{array}\right]\end{array}---\left(10\right)$

由式(10)可知，式(9)的前3行对应状态量可由Z直接得到，第4～6行对应的可观测状态组合可由Z和Z一阶导得到，第7～9行对应的可观测状态组合分析如下：

NY＝NQ₁X (11)

$\left[\begin{array}{c}Z\\ N_{21}Z+\stackrel{\·}{Z}\\ N_{31}Z+N_{32}\stackrel{\·}{Z}+N_{34}{Z}^{\left(3\right)}\\ N_{42}\stackrel{\·}{Z}+\stackrel{\·\·}{Z}+N_{44}{Z}^{\left(3\right)}\\ N_{52}\stackrel{\·}{Z}+N_{54}{Z}^{\left(3\right)}+Z^{\left(4\right)}\end{array}\right]=\left[\begin{array}{c}\mathrm{\δ}V\\ F_{\mathrm{\φ}v}\mathrm{\φ}+{\▿}^n\\ F_{\mathrm{\φ}\mathrm{\φ}}\mathrm{\φ}+{\mathrm{\ϵ}}^n\\ 0\\ 0\end{array}\right]---\left(12\right)$

由式(12)可得：

$N_{42}\stackrel{\·}{Z}+\stackrel{\·\·}{Z}+N_{44}{Z}^{\left(3\right)}=0---\left(13\right)$

即方程两边同时左乘矩阵得取其前两行记为：

${\[{\mathrm{UF}}_{v\mathrm{\φ}}{N}_{34}{Z}^{\left(3\right)}\]}_{1:2}=\left[\begin{array}{ccc}\frac{-1}{{\mathrm{\ω}}_{ie}\cos L}& 0& 0\\ 0& -1& 0\end{array}\right]N_{34}{Z}^{\left(3\right)}={\[K_{22}{N}_{34}{Z}^{\left(3\right)}\]}_{1:2}={\[U\left(-N_{42}\stackrel{\·}{Z}-\stackrel{\·\·}{Z}\right)\]}_{1:2}---\left(14\right)$

${\[MN\]}_{7:9}Y=(K_{21}{N}_{21}+K_{22}{N}_{31})Z+(K_{21}+K_{22}{N}_{32})\stackrel{\·}{Z}+K_{22}{N}_{34}{Z}^{\left(3\right)}---\left(15\right)$

取式(15)的前两行，记作：

$\begin{array}{c}{\[MN\]}_{7:8}Y={\[\left(K_{21}{N}_{21}+K_{22}{N}_{31}\right)Z\]}_{1:2}+{\[\left(K_{21}+K_{22}{N}_{32}\right)\stackrel{\·}{Z}\]}_{1:2}+{\[K_{22}{N}_{34}{Z}^{\left(3\right)}\]}_{1:2}=\\ {\[\left(K_{21}{N}_{21}+K_{22}{N}_{31}\right)Z\]}_{1:2}+{\[\left(K_{21}+K_{22}{N}_{32}\right)\stackrel{\·}{Z}\]}_{1:2}+{\[U\left(-N_{42}\stackrel{\·}{Z}-\stackrel{\·\·}{Z}\right)\]}_{1:2}\end{array}---\left(16\right)$

第7～8行对应的可观测状态组合由Z、和得到，第9行对应的可观测状态组合由Z、和Z⁽³⁾得到；需要量测量阶次信息的阶次越高对应状态所需估计的时间就越长，因此得出如下结论：除速度误差外状态组合外，(PX)_4:6的收敛速度最快，(PX)_7:8次之，而(PX)₉最慢；

步骤4，三位置自对准：

由式(8)得，若ε_E、▽_E和▽_N能够快速准确辨识出，即可求解初始失准角φ_E,φ_N,φ_U；双轴捷联惯导系统内框架能够沿惯导系统的Z轴转动，外框架沿惯导系统的Y轴转动；在初始对准时，首先控制内框架绕IMU的Z轴转动-90°，然后控制外框架绕IMU的X轴转动-90°，作为初始对准的第一个位置；按照式(9)在此位置进行改进模型的卡尔曼滤波精对准；

$C_{b1}^n=\left[\begin{array}{ccc}\cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ -\cos \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& -\cos \mathrm{\γ}\cos \mathrm{\θ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]---\left(17\right)$

${\left[\begin{array}{ccc}{\mathrm{\ϵ}}_E^I& {\mathrm{\ϵ}}_N^I& {\mathrm{\ϵ}}_U^I\end{array}\right]}^T=C_{b1}^n{\left[\begin{array}{ccc}{\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T---\left(18\right)$

${\left[\begin{array}{ccc}{\▿}_E^I& {\▿}_N^I& {\▿}_U^I\end{array}\right]}^T=C_{b1}^n{\left[\begin{array}{ccc}{\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]}^T---\left(19\right)$

其中θ，γ和ψ分别为载体的俯仰，横滚和航向角；ε_x，ε_y和ε_z分别为x，y，z方向的等效陀螺漂移，▽_x，▽_y和▽_z分别为x，y，z方向的等效加速度计零偏；

控制外框架绕IMU的X轴转动90°，作为初始对准的第二个位置；按照式(9)在此位置进行改进模型的卡尔曼滤波精对准；

$C_{b2}^{b1}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& -1\\ 0& 1& 0\end{array}\right]$

$\begin{array}{c}{C}_{b2}^n=C_{b1}^n{C}_{b2}^{b1}=\\ \left[\begin{array}{ccc}\cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ -\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]\end{array}---\left(20\right)$

${\left[\begin{array}{ccc}{\mathrm{\ϵ}}_E^{II}& {\mathrm{\ϵ}}_N^{II}& {\mathrm{\ϵ}}_U^{II}\end{array}\right]}^T=C_{b2}^n{\left[\begin{array}{ccc}{\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T---\left(21\right)$

${\left[\begin{array}{ccc}{\▿}_E^{II}& {\▿}_N^{II}& {\▿}_U^{II}\end{array}\right]}^T=C_{b2}^n{\left[\begin{array}{ccc}{\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]}^T---\left(22\right)$

控制外框架绕IMU的Z轴转动90°，作为初始对准的第三个位置；按照式(9)在此位置进行改进模型的卡尔曼滤波精对准；

$C_{b3}^{b2}=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$

$\begin{array}{c}{C}_{b3}^n=C_{b1}^n{C}_{b2}^{b1}{C}_{b3}^{b2}=\\ \left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]\end{array}---\left(23\right)$

${\left[\begin{array}{ccc}{\mathrm{\ϵ}}_E^{III}& {\mathrm{\ϵ}}_N^{III}& {\mathrm{\ϵ}}_U^{III}\end{array}\right]}^T=C_{b3}^n{\left[\begin{array}{ccc}{\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T---\left(24\right)$

${\left[\begin{array}{ccc}{\▿}_E^{III}& {\▿}_N^{III}& {\▿}_U^{III}\end{array}\right]}^T=C_{b3}^n{\left[\begin{array}{ccc}{\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]}^T---\left(25\right)$

在三个位置处均对等效北向陀螺漂移ε_N和等效天向加速度计零偏▽_U进行辨识，即和▽_x、▽_y、和▽_z得到；根据式(24)，得：

${\left[\begin{array}{ccc}{\mathrm{\ϵ}}_N^I& {\mathrm{\ϵ}}_N^{II}& {\mathrm{\ϵ}}_N^{III}\end{array}\right]}^T=A{\left[\begin{array}{ccc}{\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T---\left(26\right)$

${\left[\begin{array}{ccc}{\▿}_U^I& {\▿}_U^{II}& {\▿}_U^{III}\end{array}\right]}^T=B{\left[\begin{array}{ccc}{\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]}^T---\left(27\right)$

其中

$A=\left[\begin{array}{ccc}-\cos \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\end{array}\right]$

$B=\left[\begin{array}{ccc}-\sin \mathrm{\θ}& -\cos \mathrm{\γ}\cos \mathrm{\θ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ -\sin \mathrm{\θ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

若矩阵A满秩，此时ε_x、ε_y和ε_z由式(28)唯一确定：

${\left[\begin{array}{ccc}{\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T=A^{-1}{\left[\begin{array}{ccc}{\mathrm{\ϵ}}_N^I& {\mathrm{\ϵ}}_N^{II}& {\mathrm{\ϵ}}_N^{III}\end{array}\right]}^T---\left(28\right)$

此时，由式(29)得到：

${\mathrm{\ϵ}}_E^{III}=C{\left[\begin{array}{ccc}{\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T---\left(29\right)$

其中C＝[cosγcosψ-sinγsinθsinψ -cosθsinψ sinγcosψ+cosγsinθsinψ]；

若矩阵B满秩，此时▽_x、▽_y和▽_z由式(30)唯一确定：

${\left[\begin{array}{ccc}{\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]}^T=B^{-1}{\left[\begin{array}{ccc}{\▿}_U^I& {\▿}_U^{II}& {\▿}_U^{III}\end{array}\right]}^T---\left(30\right)$

此时，和由式(31)得到：

${\left[\begin{array}{cc}{\▿}_E^{III}& {\▿}_N^{III}\end{array}\right]}^T=D{\left[\begin{array}{ccc}{\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]}^T---\left(31\right)$

其中

在第三位置进行对准，按照估计出来的和代入式(9)估计的PX中，按照式(32)进行计算，得到对准结束时刻的失准角φ_E、φ_N和φ_U；

$\begin{array}{c}{\mathrm{\φ}}_E={\left(PX\right)}_5-\frac{1}{g}{\▿}_N\\ {\mathrm{\φ}}_N={\left(PX\right)}_6+\frac{1}{g}{\▿}_E\\ {\mathrm{\φ}}_U={\left(PX\right)}_7-\frac{1}{{\mathrm{\ω}}_{ie}\cos L}{\mathrm{\ϵ}}_E+\frac{\tan L}{g}{\▿}_E\end{array}---\left(32\right)$

2.根据权利要求1所述的带双轴转位机构捷联惯导的三位置自对准方法，其特征在于，所述步骤2中，若采用单位置对准方法时，φ_E的对准误差为φ_N的对准误差为φ_U的对准误差为根据步骤4估计出的和对φ_E、φ_N和φ_U的结果进行补偿，计算出对准结束时刻的失准角φ_E、φ_N和φ_U，然后根据式(33)对第三位置的姿态矩阵进行更新

${\stackrel{\‾}{C}}_{b3}^n=\[I+(\mathrm{\φ}\×)\]C_{b3}^n---\left(33\right)$

即为经过误差补偿后的最终姿态矩阵。