[发明专利]一种带双轴转位机构捷联惯导的三位置自对准方法

摘要

本发明公开了一种带双轴转位机构捷联惯导的三位置自对准方法，包括以下步骤：1)建立改进的卡尔曼滤波模型；2)可观测性分析；3)可观测组合的估计收敛速度分析；4)三位置自对准；本发明通过改进模型可以使捷联惯导系统的等效北向陀螺漂移和等效天向加速度计的零偏快速收敛；提出的带双轴转位机构捷联惯导三位置自对准方法，适用于捷联惯导系统在初始任意姿态角条件下的快速自对准；本发明可以有效缩短对准时间，提高带双轴转位机构的捷联惯导系统的自对准精度。

主权项

一种带双轴转位机构捷联惯导的三位置自对准方法，其特征在于，包括以下步骤：步骤1，建立改进的卡尔曼滤波模型：以东北天坐标系作为导航坐标系，其中导航坐标系用n表示、机体坐标系用b表示、惯性坐标系用i表示；ωie中的下标表示地球坐标系相对于惯性坐标系的旋转角速度，εb表示等效陀螺漂移ε在b系内的投影，▽b表示等效加速度计的零偏▽在b系内的投影；速度误差向量δVn、姿态误差向量φ、导航坐标系下的陀螺常值漂移εn和加速度计零偏▽n，其形式如下所示：δVn=δVEδVNδVU,φ=φEφNφU,▿n=▿E▿N▿U,ϵn=ϵEϵNϵU]]>在自对准过程中，载体无移动，捷联惯导系统的速度误差方程为：δV·n=Fvv·δVn+Fvφ·φ+▿n---(1)]]>其中，Fvφ＝(fn×)，“×”表示叉乘运算；捷联惯导系统的姿态误差方程为：φ·=Fφv·δVn+Fφφ·φ-ϵn---(2)]]>其中，RM、RN分别为经纬圈和卯酉圈半径，L、h分别为纬度和高度；将等效陀螺的漂移εb和等效加速度计零偏▽b视为随机常值误差，选取状态变量为：X＝[δVn φ ▽n εn]Τ，同时以速度误差δVn作为外界量测，则系统模型表示如下：X·=F·XZ=H·X+V---(3)]]>其中H＝[I 0 0 0]，I为3阶单位矩阵，0为3阶零矩阵；V为由晃动等干扰引起的量测噪声，可以近似看作白噪声序列，假设其方差为R；步骤2，可观测性分析：依据连续时间线性定常系统可观测性分析相关理论，在单一位置对式(3)所表示的系统进行可观测性分析，构造可观测性矩阵Q＝[HΤ (HF)Τ … (HF11)Τ]Τ，令Q1＝[HΤ (HF)Τ … (HF4)Τ]Τ；Q1=HHFHF2HF3HF4=I000FvvFvφI0A1A2Fvv-FvφB1B2A1-A2C1C2B1-B2---(4)]]>其中令其中N21＝‑Fvv，N31＝‑Fφv，对式(4)左乘矩阵N进行行变换，得：NQ1=HN21H+HFN31H+N32HF+N34HF3N42HF+HF2+N44HF3N52HF+N54HF3+HF4=I0000FvφI00Fφφ0-I00000000---(5)]]>由式N52HF+N54HF3+HF4＝0可得，HF4由HF和HF3线性表示，因此HFk均由HF和HF3线性表示，其中k＝4,5…11；因此rank(Q)＝rank(Q1)＝rank(NQ1)；由式(5)可得，rank(NQ1)＝9，因此得出结论：在任一固定位置，式(3)表示的系统可观测矩阵的秩为9，不满足完全可观测的条件；但是存在可观测组合，现加以分析：记则Y＝Q1X，令其中K11=00101g0-1g00,K21=-tanLg000-ωiesinLg00ωiecosLg0,K22=-1ωiecosL000-1000-1]]>MNQ1=P06×12---(6)]]>其中MNY=MNQ1X=P06×12X---(7)]]>则式(3)表示的系统的可观测组合状态如下：PX=δVn3×1▿UφE+1g▿NφN-1g▿EφU+1ωiecosLϵE-tanLg▿EϵN-ωiesinLg▿NϵU+ωiecosLg▿N---(8)]]>步骤3，可观测组合的估计收敛速度分析：由式(7)对式(8)表示的可观测组合的收敛速度进行分析；根据式(7)可知，MNY的前9阶对应可观测组合的量测，因此，仅需对MNY矩阵的前9行进行分析，过程如下：[MN]1:9Y＝[MNQ1]1:9X＝PX  (9)式中，下标1:9表示矩阵的前9行；[MN]1:9=(I00000K110000K21K2200000I00000I·I0000N21I000N31N320N3400N42IN4400N520N54I)1:9=I0000K11N21K11000K21N21+K22N31K21+K22N320K22N340---(10)]]>由式(10)可知，式(9)的前3行对应状态量可由Z直接得到，第4～6行对应的可观测状态组合可由Z和Z一阶导得到，第7～9行对应的可观测状态组合分析如下：NY＝NQ1X  (11)ZN21Z+Z·N31Z+N32Z·+N34Z(3)N42Z·+Z··+N44Z(3)N52Z·+N54Z(3)+Z(4)=δVFφvφ+▿nFφφφ+ϵn00---(12)]]>由式(12)可得：N42Z·+Z··+N44Z(3)=0---(13)]]>即方程两边同时左乘矩阵得取其前两行记为：[UFvφN34Z(3)]1:2=-1ωiecosL000-10N34Z(3)=[K22N34Z(3)]1:2=[U(-N42Z·-Z··)]1:2---(14)]]>[MN]7:9Y=(K21N21+K22N31)Z+(K21+K22N32)Z·+K22N34Z(3)---(15)]]>取式(15)的前两行，记作：[MN]7:8Y=[(K21N21+K22N31)Z]1:2+[(K21+K22N32)Z·]1:2+[K22N34Z(3)]1:2=[(K21N21+K22N31)Z]1:2+[(K21+K22N32)Z·]1:2+[U(-N42Z·-Z··)]1:2---(16)]]>第7～8行对应的可观测状态组合由Z、和得到，第9行对应的可观测状态组合由Z、和Z(3)得到；需要量测量阶次信息的阶次越高对应状态所需估计的时间就越长，因此得出如下结论：除速度误差外状态组合外，(PX)4:6的收敛速度最快，(PX)7:8次之，而(PX)9最慢；步骤4，三位置自对准：由式(8)得，若εE、▽E和▽N能够快速准确辨识出，即可求解初始失准角φE,φN,φU；双轴捷联惯导系统内框架能够沿惯导系统的Z轴转动，外框架沿惯导系统的Y轴转动；在初始对准时，首先控制内框架绕IMU的Z轴转动‑90°，然后控制外框架绕IMU的X轴转动‑90°，作为初始对准的第一个位置；按照式(9)在此位置进行改进模型的卡尔曼滤波精对准；Cb1n=cosθsinψ-sinγcosψ-cosγsinθsinψcosγcosψ-sinγsinθsinψ-cosθcosψ-sinγsinψ+cosγsinθcosψcosγsinψ+sinγsinθcosψ-sinθ-cosγcosθ-sinγcosθ---(17)]]>ϵEIϵNIϵUIT=Cb1nϵxϵyϵzT---(18)]]>▿EI▿NI▿UIT=Cb1n▿x▿y▿zT---(19)]]>其中θ，γ和ψ分别为载体的俯仰，横滚和航向角；εx，εy和εz分别为x，y，z方向的等效陀螺漂移，▽x，▽y和▽z分别为x，y，z方向的等效加速度计零偏；控制外框架绕IMU的X轴转动90°，作为初始对准的第二个位置；按照式(9)在此位置进行改进模型的卡尔曼滤波精对准；Cb2b1=10000-1010]]>Cb2n=Cb1nCb2b1=cosθsinψcosγcosψ-sinγsinθsinψsinγcosψ+cosγsinθsinψ-cosθcosψcosγsinψ+sinγsinθcosψsinγsinψ-cosγsinθcosψ-sinθ-sinγcosθcosγcosθ---(20)]]>ϵEIIϵNIIϵUIIT=Cb2nϵxϵyϵzT---(21)]]>▿EII▿NII▿UIIT=Cb2n▿x▿y▿zT---(22)]]>控制外框架绕IMU的Z轴转动90°，作为初始对准的第三个位置；按照式(9)在此位置进行改进模型的卡尔曼滤波精对准；Cb3b2=0-10100001]]>Cb3n=Cb1nCb2b1Cb3b2=cosγcosψ-sinγsinθsinψ-cosθsinψsinγcosψ+cosγsinθsinψcosγsinψ+sinγsinθcosψcosθcosψsinγsinψ-cosγsinθcosψ-sinγcosθsinθcosγcosθ---(23)]]>ϵEIIIϵNIIIϵUIIIT=Cb3nϵxϵyϵzT---(24)]]>▿EIII▿NIII▿UIIIT=Cb3n▿x▿y▿zT---(25)]]>在三个位置处均对等效北向陀螺漂移εN和等效天向加速度计零偏▽U进行辨识，即和▽x、▽y、和▽z得到；根据式(24)，得：ϵNIϵNIIϵNIIIT=AϵxϵyϵzT---(26)]]>▿UI▿UII▿UIIIT=B▿x▿y▿zT---(27)]]>其中A=-cosθcosψ-sinγsinψ+cosγsinθcosψcosγsinψ+sinγsinθcosψ-cosθcosψcosγsinψ+sinγsinθcosψsinγsinψ-cosγsinθcosψcosγsinψ+sinγsinθcosψcosθcosψsinγsinψ-cosγsinθcosψ]]>B=-sinθ-cosγcosθ-sinγcosθ-sinθ-sinγcosθcosγcosθ-sinγcosθsinθcosγcosθ]]>若矩阵A满秩，此时εx、εy和εz由式(28)唯一确定：ϵxϵyϵzT=A-1ϵNIϵNIIϵNIIIT---(28)]]>此时，由式(29)得到：ϵEIII=CϵxϵyϵzT---(29)]]>其中C＝[cosγcosψ‑sinγsinθsinψ ‑cosθsinψ sinγcosψ+cosγsinθsinψ]；若矩阵B满秩，此时▽x、▽y和▽z由式(30)唯一确定：▿x▿y▿zT=B-1▿UI▿UII▿UIIIT---(30)]]>此时，和由式(31)得到：▿EIII▿NIIIT=D▿x▿y▿zT---(31)]]>其中在第三位置进行对准，按照估计出来的和代入式(9)估计的PX中，按照式(32)进行计算，得到对准结束时刻的失准角φE、φN和φU；φE=(PX)5-1g▿NφN=(PX)6+1g▿EφU=(PX)7-1ωiecosLϵE+tanLg▿E---(32)]]>