基于李群滤波的捷联惯性导航初始对准方法与流程

本发明公开了一种基于李群滤波的捷联惯性导航初始对准方法，该方法属于导航方法及应用技术领域。

背景技术：

所谓导航，就是正确地引导载体沿着预定的航线、以要求的精度、在指定的时间内将载体引导至目的地的过程。惯性导航系统根据自身传感器的输出，以牛顿第二定律为理论基础，对载体的各项导航参数进行解算。它是一种自主式的导航系统，在工作时不依靠外界信息，也不向外界辐射任何能量，隐蔽性好、抗扰性强，能够全天时、全天候为载体提供完备的运动信息。

早期的惯导系统以平台惯导为主，随着惯性器件的成熟和计算机技术的发展，上世纪60年代开始出现了惯性器件与载体直接固联的捷联惯导系统。与平台惯导相比，捷联惯导系统省去了复杂的实体稳定平台，具有成本低、体积小、重量轻、可靠性高等优点。近年来，捷联惯导系统日趋成熟，精度逐步提高，应用范围也逐渐扩大。捷联式惯性导航技术将陀螺仪和加速度计直接安装在载体上，得到载体系下的加速度和角速度，通过导航计算机将测得的数据转换至导航坐标系完成导航，它不需要实体的稳定平台，成本低、体积小、重量轻、可靠性高。

捷联式惯性导航系统进入导航任务前要先进行初始对准，目的是建立精确的初始姿态矩阵，从而得到载体相对空间的姿态。对准时间和对准精度是初始对准的两个重要指标。对准时间反映系统的快速反应能力，对准精度反映系统的导航性能。作为惯导系统的一个关键技术，初始对准是国内外学者多年来的研究热点。惯导系统的初始对准分为粗对准和精对准两个阶段。

比较传统的初始对准方法，仅适用于静止或微幅晃动的对准环境。对于工作在复杂环境中的载体，诸如发动机处于高频振动的汽车、格斗状态下的战斗机、浪涌下的舰船等，载体的角振动和线振动会导致初始对准的精度和稳定性下降。捷联惯导系统在自对准过程中的姿态时刻都在发生变化，初始对准的时间和精度会受到影响。因此，在运载体晃动干扰环境下完成初始对准过程，就必须屏蔽这些无法消除的扰动影响。对准领域的研究重点是动态情况下捷联式惯性导航系统的初始对准，致力于研究出抗扰能力强、对准精度高、适应于各种复杂恶劣环境的对准方法。本发明将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，可有效地解决晃动条件下初始对准的问题。

四元数的表示方法弥补了欧拉角的的不足，计算过程中不存在奇异点的问题，但是四元数存在约束条件，在采用四元数描述姿态运动时，在滤波过程中由于难以满足其约束条件而出现问题。并且，采用四元数的描述方式在计算过程中表述复杂，而且难以避免的存在计算误差，导致姿态解算存在偏差。

针对上述问题，本发明将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，可有效地解决晃动条件下初始对准的问题。应用李群滤波器进行递归迭代，大大的简化了计算过程。基于SO(3)群的概率分布函数推导，由于SO(3)群是紧的，李代数滤波器有效的避免了姿态解算中的奇异之问题，可以对姿态进行全局表示。基于李群描述的滤波运算，也大大减少了矩阵和向量转化中的计算误差，提升了计算速度和计算精度。减轻了晃动干扰对初始对准的影响，使在晃动条件下仍然能够快速、精确地实现初始对准。该方法不依赖于系统的误差模型，且无需在粗对准的基础上进行精对准即可完成晃动基座下的初始对准。

技术实现要素：

基于李群滤波的初试对准方法是应用李群和李代数替代传统的四元数方法来实现捷联惯导的解算过程，并与最优姿态估计的初始对准思想相结合，将初始对准转化为姿态估计的问题，构建了基于李群结构的载体晃动干扰情况下捷联惯导系统初始对准模型，应用李群滤波器进行递归迭代快速有效地实现精对准。姿态矩阵的求解问题转化为初始时刻惯性坐标系下SO(3)群的估计问题，将初始姿态矩阵分解为三个SO(3)群的连续相乘，通过惯性元件的积分计算和地理位置信息，建立新的观测模型。采用李群和李代数描述捷联解算，应用李群滤波器进行递归迭代，求出姿态矩阵。本发明实时地反映载体在晃动干扰下的姿态变化，减轻了晃动干扰对初始对准的影响，使在晃动条件下仍然能够快速、精确地实现初始对准，不依赖于系统的误差模型，且无需在粗对准的基础上进行精对准即可完成晃动基座下的初始对准，计算简单，适应性强，能够用于复杂的随机系统，确保对准精度的同时提高了对准速度，在实际工程中具有良好的应用前景。

在本发明方法的详细描述中坐标系定义如下：地球坐标系e系，原点选取地球中心，X轴位于赤道平面内，从地心指向载体所在点经线，Z轴沿地球自转轴方向，随地球自转而转动，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；地心惯性坐标系i系，是在粗对准起始时刻将地球坐标系e系惯性凝固后形成的坐标系；导航坐标系n系，即导航基准的坐标系，导航相关运算都在该坐标系下进行，原点位于舰载机重心，X轴指向东向E，Y轴指向北向N，Z轴指向天向U；载体坐标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上。

在本方法中，根据现有的捷联惯性导航系统初始对准中的问题，提出了基于李群滤波的捷联惯性导航初试对准方法。

为实现方法流程，本发明采用的技术方案为基于李群滤波的捷联惯性导航初始对准方法，该方法通过下述流程实现，

(1)捷联惯导系统进行预热准备，启动系统，获得载体所在位置的经度λ、纬度L的基本信息，采集惯性测量单元IMU中陀螺的输出角速度信息和加速度计的输出信息f^b等，总体实物图如图1所示；

(2)对采集到的陀螺和加速度计的数据进行处理，应用李群滤波方法解算姿态矩阵。

方法的整体计算流程如2所示。

将初始对准转化为姿态估计的问题，姿态变换为两个坐标系之间的旋转变换，导航的姿态表示用一个3×3的正交变换矩阵来表示，如图3所示。导航的正交变换矩阵符合李群3维特殊正交群SO(3)的性质，构成了SO(3)群：

其中，任意转动群R∈SO(3)对应了特定的导航姿态矩阵，表示3×3的向量空间，上标T表示矩阵的转置，I表示三维单位矩阵，det(R)表示为矩阵R的行列式。

姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，将姿态矩阵分解为三个部分，姿态矩阵的分解形式如下：

其中，t表示时间变量，n(t)表示t时刻的导航坐标系，n(0)表示t₀时刻的导航坐标系，b(t)表示t时刻的载体坐标系，b(0)表示t₀时刻的载体坐标系，和分别为导航坐标系和机体坐标系下从初始t₀时刻到t时刻的姿态转换矩阵。和由陀螺和加速度计的信息计算得到。那么，初始对准的任务由求解姿态矩阵的问题转化为求解初始姿态阵的问题。为初始t₀时刻的机体系与导航系之间的姿态转换矩阵，是一个常值矩阵。

取地理坐标系为导航坐标系，惯性导航的基本方程，载体速度微分方程表示为：

其中，vⁿ表示相对于地球的载体速度；fⁿ表示为比力在导航系下的投影；f^b表示为比力在载体系下的投影，由加速度计测量得到；表示为地球坐标系相对于惯性坐标系的角速率；表示为导航坐标系相对于地球坐标系的角速率；gⁿ表示重力。

将式(2)代入式(3)得：

上式两边同时左乘姿态转换矩阵则有：

上式经整理，得到：

对式(5)的两边进行积分，得到：

式(5)的左边展开为：

其中，vⁿ(0)为初始t₀时刻的速度。

将式(8)和式(7)带入式(6)得：

简化表示为：

式(10)就表示为速率方程在惯性坐标系的积分形式，将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题。

式(12)是关于初始姿态阵的数学方程。α(t)、β(t)表示如上，由加速度计和陀螺的输出计算得到。

这样给出的是α(t)、β(t)实现的连续形式，通过α(t)、β(t)对应的积分迭代算法结解算α和β具体的值。由于在角晃动或线晃动激烈频繁的环境下，载体作姿态更新时，单子样旋转矢量法对有限转动引起的不可交换误差的补偿程度不够，造成算法漂移十分严重。而多子样旋转矢量法能实现对不可

交换误差的有效补偿，算法简单，易于操作，工程上非常实用。子样数越高，算法的精度越高，但计算量也越大。综合考虑精度要求和计算量，选择双子样旋转矢量算法对α(t)、β(t)进行积分迭代计算。

矢量α(t)近似为：

对式(13)右边的积分部分采用双子样旋转矢量法进行计算：

式(14)带入到式(13)简化得：

式(12)右边的最后一项为：

假设速度在[t_k,t_k+1]内呈线性变化，式(12)右边的倒数第二项近似为：

把式(16)和式(17)代入式(12)，化简得：

根据式(10)-式(18)，建立起系统的观测方程：

β_n＝R_nα_n+Q_v (19)

Q_v为系统观测噪声协方差阵。观测方程是由实测数据运算所得，存在误差项。

由于将求解姿态矩阵的问题转化为求解初始姿态阵的问题，在整个初始对准过程中为常值，建立系统状态方程如下：

R_n＝R_n-1 (20)

根据上述内容，将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，建立起了具有李群结构的系统方程，表示为：

采用李群的结构表示，避免了四元数的描述方式在计算过程中表述复杂和存在计算误差，并且在计算过程中也不存在奇异点的问题。但是矩阵形式的表示并不适用于常规滤波方法。采用矩阵奇异值分解(SVD)的方法求解姿态阵，但是由于矩阵奇异值分解的方法固定，灵活性和适应性较差，而且强烈的依赖于样本范围，精确性较差，受传感器数据精度影响较大，计算精度差。也可以采用最优姿态方法，建立初始对准问题与最优姿态确定问题之间的联系，运用Wahba姿态确定问题将对准问题转化为最小化求解问题，改变观测方程结构，建立拉格朗日方程，求解对应最小特征值的特征向量作为最优解，解决姿态求解问题。但是最优姿态方法计算量，变换过程表述复杂，存在计算误差，虽然一定程度上提升了计算精度和适应性，但是还是存在缺陷。采用李群滤波方式，切合系统整体结构，大量减少计算误差，以最小均方误差为估计为估计准则，可以快速有效地估计出系统姿态矩阵。

设计李群滤波器精确估计两个惯性系之间的关系，进而得到捷联姿态矩阵，完成初始对准。

建立系统的李群滤波方程：

其中，Q_w为系统状态噪声协方差阵，由于在整个初始对准过程中为常值，Q_w＝0_3×3；Q_v为系统观测噪声协方差阵；H_ξ为系统量测矩阵，H_ξ＝[α×]；为李群滤波一步预测估值；P_n为误差协方差矩阵；Δx为系统偏差量，以李群结构更新姿态阵。

根据以上述李群滤波方法进行递归迭代，求出再根据式(2)求解从而完成捷联惯导系统初试对准过程。

附图说明

图1：捷联惯性导航系统装置总体简图；

图2：捷联惯导系统流程图；

图3：导航坐标系到机体坐标系的一般运动；

图4：李群滤波方法流程图；

图5：仿真数据结果图；其中，(a)为航向误差角；(b)为俯仰误差角；(c)为横滚误差角；

图6：实测数据结果图；其中，(a)为航向误差角；(b)为俯仰误差角；(c)为横滚误差角；

图7：上位机采集真实姿态信息页面示意图；

具体实施方式

本发明是基于李群滤波的捷联惯性导航系统初试对准设计，下面结合本发明系统流程图对本发明的具体实施步骤进行详细的描述：

步骤1：系统准备阶段，导航系统初始化，获得载体所在位置的经度λ、纬度L的基本信息，采集惯性测量单元IMU中陀螺的输出角速度信息和加速度计的输出信息f^b等；

步骤2：通过更新计算

由于通常是缓慢变化的，则姿态矩阵近似为：

其中

那么得到姿态阵为：

步骤3：通过陀螺测量到的角速度更计算

姿态矩阵近似为：

其中，根据双子样算法得，

那么得到姿态阵为：

步骤4：建立相关的系统方程，即系统状态方程和系统观测方程。

取地理坐标系为导航坐标系，惯性导航的基本方程，速度微分方程表示为：

将式(2)代入式(26)，得：

整理得：

积分得：

其中，vⁿ(0)为初始t₀时刻的速度。

简化表示为：

式(30)就表示为速率方程在惯性坐标系的积分形式，将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题。

式(30)是关于初始姿态阵的数学方程。α(t)、β(t)表示如上，由加速度计和陀螺的输出计算得到。

这样给出的是α(t)、β(t)实现的连续形式，通过α(t)、β(t)对应的积分迭代算法结解算α和β具体的离散值。

化简计算得：

化简计算得：

根据上式，建立起系统的观测方程：

β_n＝R_nα_n+Q_v (35)

Q_v为系统观测噪声协方差阵。观测方程是由实测数据运算所得，存在误差项。

由于将求解姿态矩阵的问题转化为求解初始姿态阵的问题，我们知道在整个初始对准过程中为常值，建立系统状态方程如下：

R_n＝R_n-1 (36)

根据上述内容，将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，建立起了具有李群结构的系统方程，表示为：

步骤5：采用李群滤波估计

采用李群的结构表示，避免了四元数的描述方式在计算过程中表述复杂和存在计算误差的，并且在计算过程中也不存在奇异点的问题。李群滤波方式，切合系统整体结构，大量减少计算误差，以最小均方误差为估计为估计准则，可以快速有效地估计出系统姿态矩阵。

建立系统的李群滤波方程：

其中，Q_w为系统状态噪声协方差阵，由于在整个初始对准过程中为常值，Q_w＝0_3×3；Q_v为系统观测噪声协方差阵；H_ξ为系统量测矩阵，H_ξ＝[α×]；为李群滤波一步预测估值；P_n为误差协方差矩阵；Δx为系统偏差量，以李群结构更新姿态阵。最后求得的R_n就是我们需要的

根据以上述李群滤波方法进行递归迭代，求出再根据式(2)求解从而完成捷联惯导系统初试对准过程。

步骤6：求解姿态阵解算姿态。

前文将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，将姿态矩阵分解为三个部分，姿态矩阵的分解形式如下：

根据之前步骤求解的和即求解导航解姿态阵解算姿态信息。

本发明的有益效果如下：

(1)在以下仿真环境下，对该方法进行仿真实验：

舰船受风浪波动影响，其航向角ψ、俯仰角θ、横滚角γ作周期变化：

存在横荡、纵荡和垂荡引起的线速度：

ADx＝0.02m，ADy＝0.03m，ADz＝0.3m；ωDi＝2π/TDi,且TDx＝7s，TDy＝6s，TDz＝8s；为[0,2π]上服从均匀分布的随机相位；

初始地理位置：东经118°，北纬40°；

陀螺漂移：三个方向轴上的陀螺常值漂移为0.01°/h，随机漂移为0.001°/h；

加速度计零位偏置：三个方向轴上的加速度计常值偏置为1×10^-4g，随机偏置为

常数设置

赤道半径：R_e＝6378165.0m；

椭球扁率：e＝3.352e-3；

由万有引力得地球表面重力加速度：g₀＝9.78049；

地球自转角速度(单位为rad/s)：

常数：π＝3.1415926；

方法仿真结果如下：

进行了600s仿真，仿真结果如5所示。以姿态误差角的估计误差作为衡量精对准的指标。可以看得出，俯仰姿态在180s内完成精对准，稳定在0.02度；横滚姿态在40s内完成精对准，稳定在0.01度；航向姿态在180s内完成精对准，精度为0.02度以内。由仿真结果可知，本方法可以快速有效的完成解算对准过程，相比于四元数卡尔曼滤波方法，超调量明显较小，收敛速度快，而且滤波精度较好。

(2)通过真实实验对本发明提出的基于李群滤波的捷联惯导初试系统进行验证。真实试验中，不提任何供外界辅助信息，系统装置放置在车上，有人员上下车、开关车门、对车进行晃动等干扰。实验历时600s，试验地点在北京工业大学羽毛球馆南广场位置。上位导航计算机控制导航系统，以100HZ的数据更新速率，115200bps的波特率，采集航向精度达0.1度、姿态精度达0.05度的实际三轴姿态信息，其上位机采集页面示意图如图7。解算获得的载体姿态信息与本步骤中得到的高精度真实载体姿态信息做比较，证明本方法和系统的可行性和有效性。

系统实验结果如下：

截取600s的实际数据，结果如图6所示。以姿态误差角的估计误差作为衡量精对准的指标。可以看得出，俯仰姿态在180s内完成精对准，稳定在0.01度；横滚姿态在40s内完成精对准，稳定在0.01度；航向姿态在180s内完成精对准，精度为0.03度以内。本方法可以快速有效的完成解算对准过程，相比于四元数卡尔曼滤波方法，超调量较小，收敛速度明显较快，而且滤波精度较好。