[发明专利]基于李群滤波的捷联惯性导航初始对准方法

摘要

本发明公开了基于李群滤波的捷联惯性导航初始对准方法，采用李群和李代数描述捷联解算，可以有效避免四元数解算过程中的奇异值问题和归一化误差，通过惯性元件的积分计算和地理位可以置信息，建立新的系统模型。直接采用李群滤波器对SO(3)群进行递归估计，可以有效避免四元数模型的非线性问题，实现快速精确的初始对准。实时地反映载体在晃动干扰下的姿态变化，使在晃动条件下仍然能够快速、精确地实现初始对准，不依赖于系统的误差模型，且无需在粗对准的基础上进行精对准即可完成晃动基座下的初始对准，计算简单，适应性强，能够用于复杂的随机系统，确保对准精度的同时提高了对准速度，在实际工程中具有良好的应用前景。

主权项

基于李群滤波的捷联惯性导航初始对准方法，本方法的详细描述中坐标系定义如下：地球坐标系e系，原点选取地球中心，X轴位于赤道平面内，从地心指向载体所在点经线，Z轴沿地球自转轴方向，随地球自转而转动，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；地心惯性坐标系i系，是在粗对准起始时刻将地球坐标系e系惯性凝固后形成的坐标系；导航坐标系n系，即导航基准的坐标系，导航相关运算都在该坐标系下进行，原点位于舰载机重心，X轴指向东向E，Y轴指向北向N，Z轴指向天向U；载体坐标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上；其特征在于：在本方法中，根据现有的捷联惯性导航系统初始对准中的问题，提出了基于李群滤波的捷联惯性导航初试对准方法；为实现方法流程，采用的技术方案为基于李群滤波的捷联惯性导航初始对准方法，该方法通过下述流程实现，(1)捷联惯导系统进行预热准备，启动系统，获得载体所在位置的经度λ、纬度L的基本信息，采集惯性测量单元IMU中陀螺的输出角速度信息和加速度计的输出信息fb等；(2)对采集到的陀螺和加速度计的数据进行处理，应用李群滤波方法解算姿态矩阵；将初始对准转化为姿态估计的问题，姿态变换为两个坐标系之间的旋转变换，导航的姿态表示用一个3×3的正交变换矩阵来表示；导航的正交变换矩阵符合李群3维特殊正交群SO(3)的性质，构成了SO(3)群：其中，任意转动群R∈SO(3)对应了特定的导航姿态矩阵，表示3×3的向量空间，上标T表示矩阵的转置，I表示三维单位矩阵，det(R)表示为矩阵R的行列式；姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，将姿态矩阵分解为三个部分，姿态矩阵的分解形式如下：Rbn(t)=Rb(t)n(t)=Rn(0)n(t)Rbn(0)Rb(t)b(0)---(2)]]>其中，t表示时间变量，n(t)表示t时刻的导航坐标系，n(0)表示t0时刻的导航坐标系，b(t)表示t时刻的载体坐标系，b(0)表示t0时刻的载体坐标系，和分别为导航坐标系和机体坐标系下从初始t0时刻到t时刻的姿态转换矩阵；和由陀螺和加速度计的信息计算得到；那么，初始对准的任务由求解姿态矩阵的问题转化为求解初始姿态阵的问题；为初始t0时刻的机体系与导航系之间的姿态转换矩阵，是一个常值矩阵；取地理坐标系为导航坐标系，惯性导航的基本方程，载体速度微分方程表示为：v·n=fn-(2ωien+ωenn)×vn+gn=Rbnfb-(2ωien+ωenn)×vn+gn---(3)]]>其中，vn表示相对于地球的载体速度；fn表示为比力在导航系下的投影；fb表示为比力在载体系下的投影，由加速度计测量得到；表示为地球坐标系相对于惯性坐标系的角速率；表示为导航坐标系相对于地球坐标系的角速率；gn表示重力；将式(2)代入式(3)得：v·n=Rn(0)n(t)Rbn(0)Rb(t)b(0)fb-(2ωien+ωenn)×vn+gn---(4)]]>上式两边同时左乘姿态转换矩阵则有：Rn(t)n(0)v·n=Rbn(0)Rb(t)b(0)fb-Rn(t)n(0)(2ωien+ωenn)×vn+Rn(t)n(0)gn---(5)]]>上式经整理，得到：Rbn(0)Rb(t)b(0)fb=Rn(t)n(0)(v·n+(2ωien+ωenn)×vn-gn)---(6)]]>对式(5)的两边进行积分，得到：∫0tRn(t)n(0)v·ndt=Rbn(0)∫0tRb(t)b(0)fbdt-∫0tRn(t)n(0)(2ωien+ωenn)×vndt+∫0tRn(t)n(0)gndt---(7)]]>式(5)的左边展开为：∫0tRn(t)n(0)v·ndt=Rn(t)n(0)vn|0t-∫0tRn(t)n(0)ωinn×vndt=Rn(t)n(0)vn-vn(0)-∫0tRn(t)n(0)ωinn×vndt---(8)]]>其中，vn(0)为初始t0时刻的速度；将式(8)和式(7)带入式(6)得：Rbn(0)∫0tRb(t)b(0)fbdt=Rn(t)n(0)vn-vn(0)+∫0tRn(t)n(0)ωien×vndt-∫0tRn(t)n(0)gndt---(9)]]>简化表示为：Rbn(0)α(t)=β(t)---(10)]]>式(10)就表示为速率方程在惯性坐标系的积分形式，将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题；α(t)=∫0tRb(t)b(0)fbdt---(11)]]>β(t)=Rn(t)n(0)vn-vn(0)+∫0tRn(t)n(0)ωien×vndt-∫0tRn(t)n(0)gndt---(12)]]>式(12)是关于初始姿态阵的数学方程；α(t)、β(t)表示如上，由加速度计和陀螺的输出计算得到；这样给出的是α(t)、β(t)实现的连续形式，通过α(t)、β(t)对应的积分迭代算法结解算α和β具体的值；由于在角晃动或线晃动激烈频繁的环境下，载体作姿态更新时，单子样旋转矢量法对有限转动引起的不可交换误差的补偿程度不够，造成算法漂移十分严重；而多子样旋转矢量法能实现对不可交换误差的有效补偿，算法简单，易于操作，工程上非常实用；子样数越高，算法的精度越高，但计算量也越大；综合考虑精度要求和计算量，选择双子样旋转矢量算法对α(t)、β(t)进行积分迭代计算；矢量α(t)近似为：α(t)=∫0tRb(t)b(0)fbdt=Σk=0M-1Rb(tk)b(0)∫tktk+1Rb(t)b(tk)fbdt≈Σk=0M-1Rb(tk)b(0)∫tktk+1(I+(∫tktk+1ωibbdτ))fbdt---(13)]]>对式(13)右边的积分部分采用双子样旋转矢量法进行计算：∫tktk+1(I+(∫tktk+1ωibbdτ))fbdt=Δv1+Δv2+12(Δθ1+Δθ2)×(Δv1+Δv2)+23(Δθ1×Δv2+Δv1×Δθ2)---(14)]]>式(14)带入到式(13)简化得：α(tM)=Σk=0M-1Rb(tk-1)b(0)[Δv1+Δv2+12(Δθ1+Δθ2)×(Δv1+Δv2)+23(Δθ1×Δv2+Δv1×Δθ2)]---(15)]]>式(12)右边的最后一项为：∫0tRn(t)n(0)gndt=Σk=0M-1∫tktk-1Rn(t)n(0)gndt=Σk=0M-1Rn(tk)n(0)∫tktk-1Rn(t)n(tk)gndt≈Σk=0M-1Rn(tk)n(0)(TI+T22ωinn×)gn---(16)]]>假设速度在[tk,tk+1]内呈线性变化，式(12)右边的倒数第二项近似为：∫0tRn(t)n(0)ωien×vndt=Σk=0M-1Rn(tk)n(0)∫tktk-1Rn(t)n(tk)ωien×vndt≈Σk=0M-1Rn(tk)n(0)[(T2I+T26ωinn×)ωien×vn(tk)+(T2I+T23ωinn×)ωien×vn(tk+1)]---(17)]]>把式(16)和式(17)代入式(12)，化简得：β(tM)=Rn(t)n(0)vn-vn(0)+Σk=0M-1Rn(tk)n(0)[(T2I+T26ωinn×)ωien×vn(tk)+(T2I+T23ωinn×)ωien×vn(tk+1)-(TI+T22ωinn×)gn]---(18)]]>根据式(10)‑式(18)，建立起系统的观测方程：βn＝Rnαn+Qv             (19)Qv为系统观测噪声协方差阵；观测方程是由实测数据运算所得，存在误差项；由于将求解姿态矩阵的问题转化为求解初始姿态阵的问题，在整个初始对准过程中为常值，建立系统状态方程如下：Rn＝Rn‑1           (20)根据上述内容，将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，建立起了具有李群结构的系统方程，表示为：Rn=Rn-1βn=Rnαn+Qv---(21)]]>采用李群的结构表示，避免了四元数的描述方式在计算过程中表述复杂和存在计算误差，并且在计算过程中也不存在奇异点的问题；但是矩阵形式的表示并不适用于常规滤波方法；采用矩阵奇异值分解(SVD)的方法求解姿态阵，但是由于矩阵奇异值分解的方法固定，灵活性和适应性较差，而且强烈的依赖于样本范围，精确性较差，受传感器数据精度影响较大，计算精度差；也可以采用最优姿态方法，建立初始对准问题与最优姿态确定问题之间的联系，运用Wahba姿态确定问题将对准问题转化为最小化求解问题，改变观测方程结构，建立拉格朗日方程，求解对应最小特征值的特征向量作为最优解，解决姿态求解问题；但是最优姿态方法计算量，变换过程表述复杂，存在计算误差，虽然一定程度上提升了计算精度和适应性，但是还是存在缺陷；采用李群滤波方式，切合系统整体结构，大量减少计算误差，以最小均方误差为估计为估计准则，快速有效地估计出系统姿态矩阵；设计李群滤波器精确估计两个惯性系之间的关系，进而得到捷联姿态矩阵，完成初始对准；建立系统的李群滤波方程：其中，Qw为系统状态噪声协方差阵，由于在整个初始对准过程中为常值，Qw＝03×3；Qv为系统观测噪声协方差阵；Hξ为系统量测矩阵，Hξ＝[α×]；为李群滤波一步预测估值；Pn为误差协方差矩阵；Δx为系统偏差量，以李群结构更新姿态阵；根据以上述李群滤波方法进行递归迭代，求出再根据式(2)求解从而完成捷联惯导系统初试对准过程。