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

技术特征：

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

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

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

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

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

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

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

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

$R_b^n\left(t\right)=R_{b\left(t\right)}^{n\left(t\right)}=R_{n\left(0\right)}^{n\left(t\right)}{R}_b^n\left(0\right)R_{b\left(t\right)}^{b\left(0\right)}---\left(2\right)$

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

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

${\stackrel{\·}{v}}^n=f^n-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v^n+g^n=R_b^n{f}^b-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v^n+g^n---\left(3\right)$

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

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

${\stackrel{\·}{v}}^n=R_{n\left(0\right)}^{n\left(t\right)}{R}_b^n\left(0\right)R_{b\left(t\right)}^{b\left(0\right)}{f}^b-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v^n+g^n---\left(4\right)$

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

$R_{n\left(t\right)}^{n\left(0\right)}{\stackrel{\·}{v}}^n=R_b^n\left(0\right)R_{b\left(t\right)}^{b\left(0\right)}{f}^b-R_{n\left(t\right)}^{n\left(0\right)}(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v^n+R_{n\left(t\right)}^{n\left(0\right)}{g}^n---\left(5\right)$

上式经整理，得到：

$R_b^n\left(0\right)R_{b\left(t\right)}^{b\left(0\right)}{f}^b=R_{n\left(t\right)}^{n\left(0\right)}({\stackrel{\·}{v}}^n+(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v^n-g^n)---\left(6\right)$

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

${\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}{\stackrel{\·}{v}}^{n}dt=R_b^n\left(0\right){\∫}_0^t{R}_{b\left(t\right)}^{b\left(0\right)}{f}^{b}dt-{\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}\left(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n\right)\×v^{n}dt+{\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}{g}^{n}dt---\left(7\right)$

式(5)的左边展开为：

${\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}{\stackrel{\·}{v}}^{n}dt=R_{n\left(t\right)}^{n\left(0\right)}{v}^n{|}_0^t-{\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}{\mathrm{\ω}}_{in}^n\×v^{n}dt=R_{n\left(t\right)}^{n\left(0\right)}{v}^n-v^n\left(0\right)-{\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}{\mathrm{\ω}}_{in}^n\×v^{n}dt---\left(8\right)$

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

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

$R_b^n\left(0\right){\∫}_0^t{R}_{b\left(t\right)}^{b\left(0\right)}{f}^{b}dt=R_{n\left(t\right)}^{n\left(0\right)}{v}^n-v^n\left(0\right)+{\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}{\mathrm{\ω}}_{ie}^n\×v^{n}dt-{\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}{g}^{n}dt---\left(9\right)$

简化表示为：

$R_b^n\left(0\right)\mathrm{\α}\left(t\right)=\mathrm{\β}\left(t\right)---\left(10\right)$

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

$\mathrm{\α}\left(t\right)={\∫}_0^t{R}_{b\left(t\right)}^{b\left(0\right)}{f}^{b}dt---\left(11\right)$

$\mathrm{\β}\left(t\right)=R_{n\left(t\right)}^{n\left(0\right)}{v}^n-v^n\left(0\right)+{\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}{\mathrm{\ω}}_{ie}^n\×v^{n}dt-{\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}{g}^{n}dt---\left(12\right)$

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

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

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

矢量α(t)近似为：

$\begin{array}{c}\mathrm{\α}\left(t\right)={\∫}_0^t{R}_{b\left(t\right)}^{b\left(0\right)}{f}^{b}dt\\ =\underset{k=0}{\overset{M-1}{\mathrm{\Σ}}}{R}_{b\left(t_k\right)}^{b\left(0\right)}{\∫}_{t_k}^{t_{k+1}}{R}_{b\left(t\right)}^{b\left(t_k\right)}{f}^{b}dt\\ \≈\underset{k=0}{\overset{M-1}{\mathrm{\Σ}}}{R}_{b\left(t_k\right)}^{b\left(0\right)}{\∫}_{t_k}^{t_{k+1}}\left(I+\left({\∫}_{t_k}^{t_{k+1}}{\mathrm{\ω}}_{ib}^{b}d\mathrm{\τ}\right)\right)f^{b}dt\end{array}---\left(13\right)$

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

$\begin{array}{c}{\∫}_{t_k}^{t_{k+1}}\left(I+\left({\∫}_{t_k}^{t_{k+1}}{\mathrm{\ω}}_{ib}^{b}d\mathrm{\τ}\right)\right)f^{b}dt\\ ={\mathrm{\Δv}}_1+{\mathrm{\Δv}}_2+\frac{1}{2}\left({\mathrm{\Δ\θ}}_1+{\mathrm{\Δ\θ}}_2\right)\×\left({\mathrm{\Δv}}_1+{\mathrm{\Δv}}_2\right)+\frac{2}{3}\left({\mathrm{\Δ\θ}}_1\×{\mathrm{\Δv}}_2+{\mathrm{\Δv}}_1\×{\mathrm{\Δ\θ}}_2\right)\end{array}---\left(14\right)$

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

$\begin{array}{c}\mathrm{\α}\left(t_M\right)=\\ \underset{k=0}{\overset{M-1}{\mathrm{\Σ}}}{R}_{b\left(t_{k-1}\right)}^{b\left(0\right)}\[{\mathrm{\Δv}}_1+{\mathrm{\Δv}}_2+\frac{1}{2}({\mathrm{\Δ\θ}}_1+{\mathrm{\Δ\θ}}_2)\×({\mathrm{\Δv}}_1+{\mathrm{\Δv}}_2)+\frac{2}{3}({\mathrm{\Δ\θ}}_1\×{\mathrm{\Δv}}_2+{\mathrm{\Δv}}_1\×{\mathrm{\Δ\θ}}_2)\]\end{array}---\left(15\right)$

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

$\begin{array}{c}{\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}{g}^{n}dt=\underset{k=0}{\overset{M-1}{\mathrm{\Σ}}}{\∫}_{t_k}^{t_{k-1}}{R}_{n\left(t\right)}^{n\left(0\right)}{g}^{n}dt\\ =\underset{k=0}{\overset{M-1}{\mathrm{\Σ}}}{R}_{n\left(t_k\right)}^{n\left(0\right)}{\∫}_{t_k}^{t_{k-1}}{R}_{n\left(t\right)}^{n\left(t_k\right)}{g}^{n}dt\\ \≈\underset{k=0}{\overset{M-1}{\mathrm{\Σ}}}{R}_{n\left(t_k\right)}^{n\left(0\right)}\left(TI+\frac{T^2}{2}{\mathrm{\ω}}_{in}^n\×\right)g^n\end{array}---\left(16\right)$

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

$\begin{array}{c}{\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}{\mathrm{\ω}}_{ie}^n\×v^{n}dt=\underset{k=0}{\overset{M-1}{\mathrm{\Σ}}}{R}_{n\left(t_k\right)}^{n\left(0\right)}{\∫}_{t_k}^{t_{k-1}}{R}_{n\left(t\right)}^{n\left(t_k\right)}{\mathrm{\ω}}_{ie}^n\×v^{n}dt\\ \≈\underset{k=0}{\overset{M-1}{\mathrm{\Σ}}}{R}_{n\left(t_k\right)}^{n\left(0\right)}\[\left(\frac{T}{2}I+\frac{T^2}{6}{\mathrm{\ω}}_{in}^n\×\right){\mathrm{\ω}}_{ie}^n\×v^n\left(t_k\right)+\left(\frac{T}{2}I+\frac{T^2}{3}{\mathrm{\ω}}_{in}^n\×\right){\mathrm{\ω}}_{ie}^n\×v^n\left(t_{k+1}\right)\]\end{array}---\left(17\right)$

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

$\mathrm{\β}\left(t_M\right)=R_{n\left(t\right)}^{n\left(0\right)}{v}^n-v^n\left(0\right)+\underset{k=0}{\overset{M-1}{\Σ}}\begin{array}{c}{R}_{n\left(t_k\right)}^{n\left(0\right)}\[\left(\frac{T}{2}I+\frac{T^2}{6}{\mathrm{\ω}}_{in}^n\×\right){\mathrm{\ω}}_{ie}^n\×v^n\left(t_k\right)\\ +(\frac{T}{2}I+\frac{T^2}{3}{\mathrm{\ω}}_{in}^n\×){\mathrm{\ω}}_{ie}^n\×v^n\left(t_{k+1}\right)-(TI+\frac{T^2}{2}{\mathrm{\ω}}_{in}^n\×)g^n\]\end{array}---\left(18\right)$

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

β_n＝R_nα_n+Q_v (19)

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

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

R_n＝R_n-1 (20)

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

$\left\{\begin{array}{c}{R}_n=R_{n-1}\\ {\mathrm{\β}}_n=R_n{\mathrm{\α}}_n+Q_v\end{array}\right.---\left(21\right)$

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

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

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

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

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

2.根据权利要求1所述的基于李群滤波的捷联惯性导航初始对准方法，其特征在于：步骤1：系统准备阶段，导航系统初始化，获得载体所在位置的经度λ、纬度L的基本信息，采集惯性测量单元IMU中陀螺的输出角速度信息和加速度计的输出信息f^b；

步骤2：通过更新计算

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

其中

那么得到姿态阵为：

$R_{n\left(0\right)}^{n\left(t_M\right)}=R_{n\left(t_{M-1}\right)}^{n\left(t_M\right)}{R}_{n\left(0\right)}^{n\left(t_{M-1}\right)}---\left(24\right)$

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

姿态矩阵近似为：

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

那么得到姿态阵为：

$R_{b\left(0\right)}^{b\left(t_M\right)}=R_{b\left(t_{M-1}\right)}^{b\left(t_M\right)}{R}_{b\left(0\right)}^{b\left(t_{M-1}\right)}---\left(26\right)$

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

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

${\stackrel{\·}{v}}^n=f^n-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v^n+g^n=R_b^n{f}^b-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v^n+g^n---\left(26\right)$

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

${\stackrel{\·}{v}}^n=R_{n\left(0\right)}^{n\left(t\right)}{R}_b^n\left(0\right)R_{b\left(t\right)}^{b\left(0\right)}{f}^b-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v^n+g^n---\left(27\right)$

整理得：

$R_b^n\left(0\right)R_{b\left(t\right)}^{b\left(0\right)}{f}^b=R_{n\left(t\right)}^{n\left(0\right)}({\stackrel{\·}{v}}^n+(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v^n-g^n)---\left(28\right)$

积分得：

$R_b^n\left(0\right){\∫}_0^t{R}_{b\left(t\right)}^{b\left(0\right)}{f}^{b}dt=R_{n\left(t\right)}^{n\left(0\right)}{v}^n-v^n\left(0\right)+{\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}{\mathrm{\ω}}_{ie}^n\×v^{n}dt-{\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}{g}^{n}dt---\left(29\right)$

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

简化表示为：

$R_b^n\left(0\right)\mathrm{\α}\left(t\right)=\mathrm{\β}\left(t\right)---\left(30\right)$

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

$\mathrm{\α}\left(t\right)={\∫}_0^t{R}_{b\left(t\right)}^{b\left(0\right)}{f}^{b}dt---\left(31\right)$

$\mathrm{\β}\left(t\right)=R_{n\left(t\right)}^{n\left(0\right)}{v}^n-v^n\left(0\right)+{\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}{\mathrm{\ω}}_{ie}^n\×v^{n}dt-{\∫}_0^t{R}_{n\left(t\right)}^{n\left(0\right)}{g}^{n}dt---\left(32\right)$

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

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

化简计算得：

$\begin{array}{c}\mathrm{\α}\left(t_M\right)=\\ \underset{k=0}{\overset{M-1}{\mathrm{\Σ}}}{R}_{b\left(t_{k-1}\right)}^{b\left(0\right)}\[{\mathrm{\Δv}}_1+{\mathrm{\Δv}}_2+\frac{1}{2}({\mathrm{\Δ\θ}}_1+{\mathrm{\Δ\θ}}_2)\×({\mathrm{\Δv}}_1+{\mathrm{\Δv}}_2)+\frac{2}{3}({\mathrm{\Δ\θ}}_1\×{\mathrm{\Δv}}_2+{\mathrm{\Δv}}_1\×{\mathrm{\Δ\θ}}_2)\]\end{array}---\left(33\right)$

化简计算得：

$\mathrm{\β}\left(t_M\right)=R_{n\left(t\right)}^{n\left(0\right)}{v}^n-v^n\left(0\right)+\underset{k=0}{\overset{M-1}{\Σ}}\begin{array}{c}{R}_{n\left(t_k\right)}^{n\left(0\right)}\[\left(\frac{T}{2}I+\frac{T^2}{6}{\mathrm{\ω}}_{in}^n\×\right){\mathrm{\ω}}_{ie}^n\×v^n\left(t_k\right)\\ +(\frac{T}{2}I+\frac{T^2}{3}{\mathrm{\ω}}_{in}^n\×){\mathrm{\ω}}_{ie}^n\×v^n\left(t_{k+1}\right)-\left(TI+\frac{T^2}{2}{\mathrm{\ω}}_{in}^n\×\right)g^n\]\end{array}---\left(34\right)$

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

β_n＝R_nα_n+Q_v (35)

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

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

R_n＝R_n-1 (36)

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

$\left\{\begin{array}{c}{R}_n=R_{n-1}\\ {\mathrm{\β}}_n=R_n{\mathrm{\α}}_n+Q_v\end{array}\right.---\left(37\right)$

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

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

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

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

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

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

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

$R_b^n\left(t\right)=R_{b\left(t\right)}^{n\left(t\right)}=R_{n\left(0\right)}^{n\left(t\right)}{R}_b^n\left(0\right)R_{b\left(t\right)}^{b\left(0\right)}---\left(39\right)$

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